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

Merge remote-tracking branch 'origin/develop' into feature/tighteningTraits 

Conflicts:
	gtsam/base/LieScalar.h
	gtsam/geometry/Point2.h
release/4.3a0
dellaert 2014-12-21 14:39:23 +01:00
parent f939723ca4 30afbc5f1d
commit 00b374c9e9
91 changed files with 2956 additions and 436 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -592,6 +592,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>
@ -599,7 +600,6 @@
</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>
@ -647,7 +647,6 @@
</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>
@ -655,7 +654,6 @@
</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>
@ -663,7 +661,6 @@
</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>
@ -679,7 +676,6 @@
</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>
@ -1029,6 +1025,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testMagFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testMagFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1135,7 +1139,6 @@
</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>
@ -1365,46 +1368,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</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>
@ -1487,6 +1450,7 @@
</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>
@ -1526,6 +1490,7 @@
</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>
@ -1533,6 +1498,7 @@
</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>
@ -1546,6 +1512,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1803,7 +1809,6 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1811,7 +1816,6 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1819,7 +1823,6 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1827,7 +1830,6 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2706,7 +2708,6 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2714,7 +2715,6 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2722,7 +2722,6 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3274,6 +3273,7 @@
</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>

1
.gitignore vendored
View File

@ -1,5 +1,4 @@
/build*
/doc*
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt

6
.settings/org.eclipse.cdt.core.prefs Normal file
View File

@ -0,0 +1,6 @@
eclipse.preferences.version=1
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=$PATH\:/opt/local/bin
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

6
CppUnitLite/Test.h
View File

@ -80,6 +80,12 @@ protected:
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
/**
* Use this to disable unwanted tests without commenting them out.
*/
#define TEST_DISABLED(testGroup, testName)\
void testGroup##testName##Test(TestResult& result_, const std::string& name_)
/*
* Convention for tests:
* - "EXPECT" is a test that will not end execution of the series of tests

607
doc/Mathematica/dexpInvL_SE2.nb Normal file
View File

@ -0,0 +1,607 @@
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 8.0' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 157, 7]
NotebookDataLength[ 18933, 598]
NotebookOptionsPosition[ 18110, 565]
NotebookOutlinePosition[ 18464, 581]
CellTagsIndexPosition[ 18421, 578]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell[TextData[{
"The \[OpenCurlyQuote]right trivialised\[CloseCurlyQuote] tangent of the \
exponential map, ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
", according to Iserles05an, formula 2.42, pg. 32 can be written as\n\t",
Cell[BoxData[
FormBox[GridBox[{
{"\t"},
{
RowBox[{
RowBox[{
RowBox[{"g", "'"}],
SuperscriptBox["g",
RowBox[{"-", "1"}]]}], "=",
RowBox[{
SubscriptBox["dexpR", "\[Omega]"], "(",
RowBox[{"\[Omega]", "'"}], ")"}]}]}
}], TraditionalForm]],
FormatType->"TraditionalForm"],
"\nwhere ",
Cell[BoxData[
FormBox[
RowBox[{"g", "=",
RowBox[{"exp", "(", "\[Omega]", ")"}]}], TraditionalForm]],
FormatType->"TraditionalForm"],
", and ",
Cell[BoxData[
FormBox[
RowBox[{
RowBox[{"g", "'"}], "=",
RowBox[{
RowBox[{"exp", "'"}],
RowBox[{"(", "\[Omega]", ")"}]}]}], TraditionalForm]],
FormatType->"TraditionalForm"],
".\nCompare this to the left Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
" in Chirikjian11book2, pg. 26, we see that ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
" and ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
" are the same.\n\nHence, be careful, Iserles\[CloseCurlyQuote]s \
\[OpenCurlyQuote]",
StyleBox["right",
FontWeight->"Bold"],
" trivialised\[CloseCurlyQuote] tangent of the exponential map ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
" is in fact Chirikjian\[CloseCurlyQuote]s ",
StyleBox["left",
FontWeight->"Bold"],
" Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
"!!!\n\nWe want to compute the s \[OpenCurlyQuote]",
StyleBox["left",
FontWeight->"Bold"],
" trivialised\[CloseCurlyQuote] tangent of the exponential map, ",
Cell[BoxData[
FormBox["dexpL", TraditionalForm]],
FormatType->"TraditionalForm"],
", for SE2, hence, we need to use Chirikjian\[CloseCurlyQuote]s ",
StyleBox["right",
FontWeight->"Bold"],
" Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "r"], TraditionalForm]],
FormatType->"TraditionalForm"],
" formula in Chirikjian11book2, pg. 36."
}], "Text",
CellChangeTimes->{{3.6279967389044943`*^9, 3.6279968865058002`*^9}, {
3.6279969695759087`*^9, 3.6279974871811028`*^9}, 3.62799757389325*^9}],
Cell[BoxData[{
RowBox[{"Clear", "[", "J", "]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"J", "[", "\[Alpha]_", "]"}], ":=",
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",",
RowBox[{
RowBox[{"(",
RowBox[{"1", "-",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], "/", "\[Alpha]"}],
",", " ",
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"\[Alpha]", " ",
SubscriptBox["v", "1"]}], "-",
SubscriptBox["v", "2"], "+",
RowBox[{
SubscriptBox["v", "2"],
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-",
RowBox[{
SubscriptBox["v", "1"],
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/",
SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",",
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{"-",
RowBox[{"(",
RowBox[{"1", "-",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}], "/", "\[Alpha]"}],
",",
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", " ",
RowBox[{
RowBox[{"(",
RowBox[{
SubscriptBox["v", "1"], "+",
RowBox[{"\[Alpha]", " ",
SubscriptBox["v", "2"]}], "-",
RowBox[{
SubscriptBox["v", "1"],
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-",
RowBox[{
SubscriptBox["v", "2"],
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/",
SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], "Input",
CellChangeTimes->{{3.627993817228732*^9, 3.6279939547434673`*^9}, {
3.627993986274671*^9, 3.6279940386007967`*^9}, {3.627995391081044*^9,
3.627995412846286*^9}, 3.6279954452391644`*^9, {3.627995531089571*^9,
3.6279955341932592`*^9}, {3.627996429604282*^9, 3.62799643077184*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]_", "]"}], "=",
RowBox[{"Inverse", "[",
RowBox[{"J", "[", "\[Alpha]", "]"}], "]"}]}],
"\[IndentingNewLine]"}]], "Input",
CellChangeTimes->{
3.627995475343099*^9, {3.627995548533964*^9, 3.627995559455151*^9}, {
3.627996438504909*^9, 3.6279964431657553`*^9}}],
Cell[BoxData[
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{
FractionBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}],
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",",
FractionBox[
RowBox[{
RowBox[{"-",
FractionBox["1", "\[Alpha]"]}], "+",
FractionBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]], ",",
FractionBox[
RowBox[{
FractionBox[
SubscriptBox["v", "1"],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
SubscriptBox["v", "2"],
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "2"]]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",",
RowBox[{"{",
RowBox[{
FractionBox[
RowBox[{
FractionBox["1", "\[Alpha]"], "-",
FractionBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]], ",",
FractionBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}],
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",",
FractionBox[
RowBox[{
RowBox[{"-",
FractionBox[
SubscriptBox["v", "1"],
SuperscriptBox["\[Alpha]", "2"]]}], "+",
FractionBox[
RowBox[{
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SubscriptBox["v", "2"],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]], "Output",
CellChangeTimes->{
3.627995560030972*^9, {3.627996412919798*^9, 3.627996444306521*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]", "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.627993835637863*^9, 3.627993839233502*^9}, {
3.627994046108457*^9, 3.627994058781851*^9}, {3.627995546842499*^9,
3.6279955664940767`*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{
RowBox[{
FractionBox["1", "2"], " ", "\[Alpha]", " ",
RowBox[{"Cot", "[",
FractionBox["\[Alpha]", "2"], "]"}]}],
RowBox[{"-",
FractionBox["\[Alpha]", "2"]}],
FractionBox[
RowBox[{
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"-", "2"}], "+",
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "1"]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], " ",
SubscriptBox["v", "2"]}]}],
RowBox[{"2", " ", "\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]},
{
FractionBox["\[Alpha]", "2"],
RowBox[{
FractionBox["1", "2"], " ", "\[Alpha]", " ",
RowBox[{"Cot", "[",
FractionBox["\[Alpha]", "2"], "]"}]}],
FractionBox[
RowBox[{
RowBox[{
RowBox[{"(",
RowBox[{"\[Alpha]", "-",
RowBox[{"\[Alpha]", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "1"]}], "+",
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"-", "2"}], "+",
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "2"]}]}],
RowBox[{"2", " ", "\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{
3.627993840513033*^9, {3.62799404156531*^9, 3.6279940592345743`*^9},
3.627995567356995*^9, 3.627996415136314*^9, 3.6279964490074778`*^9}]
}, Open ]],
Cell[TextData[{
"In case ",
Cell[BoxData[
FormBox[
RowBox[{"\[Alpha]", "=", "0"}], TraditionalForm]],
FormatType->"TraditionalForm"],
", we compute the limits of ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "r"], TraditionalForm]],
FormatType->"TraditionalForm"],
" and ",
Cell[BoxData[
FormBox[
SuperscriptBox[
SubscriptBox["J", "r"],
RowBox[{"-", "1"}]], TraditionalForm]],
FormatType->"TraditionalForm"],
" as follows"
}], "Text",
CellChangeTimes->{{3.627997495449997*^9, 3.627997524522543*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Limit", "[",
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]", "]"}], ",",
RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.627995572179821*^9, 3.627995606373824*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{"1", "0",
FractionBox[
SubscriptBox["v", "2"], "2"]},
{"0", "1",
RowBox[{"-",
FractionBox[
SubscriptBox["v", "1"], "2"]}]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{{3.627995585954463*^9, 3.627995606858135*^9},
3.6279964178087473`*^9, 3.6279964634008904`*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Limit", "[",
RowBox[{
RowBox[{"J", "[", "\[Alpha]", "]"}], ",",
RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.6279956527343893`*^9, 3.627995660697241*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{"1", "0",
RowBox[{"-",
FractionBox[
SubscriptBox["v", "2"], "2"]}]},
{"0", "1",
FractionBox[
SubscriptBox["v", "1"], "2"]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{{3.627995653969245*^9, 3.627995661346282*^9},
3.627996419383007*^9, 3.627996465705708*^9}]
}, Open ]],
Cell[BoxData[""], "Input",
CellChangeTimes->{{3.627995694633294*^9, 3.627995695945466*^9}}]
},
WindowSize->{654, 852},
WindowMargins->{{Automatic, 27}, {Automatic, 0}},
FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \
2011)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)
(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[557, 20, 2591, 84, 197, "Text"],
Cell[3151, 106, 2022, 56, 68, "Input"],
Cell[CellGroupData[{
Cell[5198, 166, 343, 9, 43, "Input"],
Cell[5544, 177, 6519, 190, 290, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[12100, 372, 298, 7, 27, "Input"],
Cell[12401, 381, 2665, 76, 99, "Output"]
}, Open ]],
Cell[15081, 460, 535, 20, 29, "Text"],
Cell[CellGroupData[{
Cell[15641, 484, 297, 8, 27, "Input"],
Cell[15941, 494, 863, 25, 91, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[16841, 524, 296, 8, 27, "Input"],
Cell[17140, 534, 859, 25, 91, "Output"]
}, Open ]],
Cell[18014, 562, 92, 1, 27, "Input"]
}
]
*)
(* End of internal cache information *)

33
gtsam.h
View File

@ -156,12 +156,6 @@ virtual class Value {
size_t dim() const;
};
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h>
class LieScalar {
// Standard constructors
@ -1230,6 +1224,7 @@ class VectorValues {
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
@ -1653,6 +1648,7 @@ class NonlinearFactorGraph {
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
@ -1723,6 +1719,7 @@ class Values {
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
@ -1733,10 +1730,14 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
// Fixed size version
void insertFixed(size_t j, Vector t, size_t n);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
@ -1753,6 +1754,16 @@ class Values {
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
// Actually a FastList<Key>
@ -2149,7 +2160,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2394,7 +2405,7 @@ class ConstantBias {
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
@ -2433,7 +2444,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
@ -2479,7 +2490,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
#include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
@ -2530,7 +2541,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
};

5
gtsam/base/LieMatrix.h
View File

@ -19,7 +19,12 @@
#include <cstdarg>
#ifdef _MSC_VER
#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
#else
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>

8
gtsam/base/LieScalar.h
View File

@ -17,8 +17,12 @@
#pragma once
// TODO(ASL) reenable
//#warning "LieScalar.h is deprecated. Please use double/float instead."
#ifdef _MSC_VER
#pragma message("LieScalar.h is deprecated. Please use double/float instead.")
#else
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
#include <gtsam/dllexport.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>

5
gtsam/base/LieVector.h
View File

@ -17,7 +17,12 @@
#pragma once
#ifdef _MSC_VER
#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.")
#else
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
#include <gtsam/base/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>

6
gtsam/base/Vector.cpp
View File

@ -286,11 +286,15 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
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)
for (size_t i = 0; i < m; ++i) {
if (weights[i] == inf && !isZero[i]) {
// Basically, instead of doing a normal QR step with the weighted
// pseudoinverse, we enforce the constraint by turning
// ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
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)

2
gtsam/base/Vector.h
View File

@ -34,6 +34,7 @@ namespace gtsam {
typedef Eigen::VectorXd Vector;
// Commonly used fixed size vectors
typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
@ -42,6 +43,7 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8;
typedef Eigen::Matrix<double, 9, 1> Vector9;
typedef Eigen::Matrix<double, 10, 1> Vector10;
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;

25
gtsam/base/debug.cpp
View File

@ -17,9 +17,32 @@
*/
#include <gtsam/base/debug.h>
#ifdef GTSAM_USE_TBB
#include <tbb/mutex.h>
#endif
namespace gtsam {
GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool, false> > debugFlags;
#ifdef GTSAM_USE_TBB
tbb::mutex debugFlagsMutex;
#endif
/* ************************************************************************* */
bool guardedIsDebug(const std::string& s) {
#ifdef GTSAM_USE_TBB
tbb::mutex::scoped_lock lock(debugFlagsMutex);
#endif
return gtsam::debugFlags[s];
}
/* ************************************************************************* */
void guardedSetDebug(const std::string& s, const bool v) {
#ifdef GTSAM_USE_TBB
tbb::mutex::scoped_lock lock(debugFlagsMutex);
#endif
gtsam::debugFlags[s] = v;
}
}

8
gtsam/base/debug.h
View File

@ -43,6 +43,10 @@
namespace gtsam {
GTSAM_EXTERN_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
// Non-guarded use led to crashes, and solved in commit cd35db2
bool GTSAM_EXPORT guardedIsDebug(const std::string& s);
void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v);
}
#undef ISDEBUG
@ -50,8 +54,8 @@ namespace gtsam {
#ifdef GTSAM_ENABLE_DEBUG
#define ISDEBUG(S) (gtsam::debugFlags[S])
#define SETDEBUG(S,V) ((void)(gtsam::debugFlags[S] = (V)))
#define ISDEBUG(S) (gtsam::guardedIsDebug(S))
#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V)))
#else

2
gtsam/geometry/Cal3Bundler.cpp
View File

@ -49,7 +49,7 @@ Vector4 Cal3Bundler::k() const {
/* ************************************************************************* */
Vector3 Cal3Bundler::vector() const {
return (Vector(3) << f_, k1_, k2_).finished();
return Vector3(f_, k1_, k2_);
}
/* ************************************************************************* */

6
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -35,8 +35,10 @@ Matrix3 Cal3DS2_Base::K() const {
}
/* ************************************************************************* */
Vector Cal3DS2_Base::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished();
Vector9 Cal3DS2_Base::vector() const {
Vector9 v;
v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_;
return v;
}
/* ************************************************************************* */

4
gtsam/geometry/Cal3DS2_Base.h
View File

@ -46,8 +46,8 @@ protected:
public:
Matrix3 K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
Vector9 vector() const ;
/// @name Standard Constructors
/// @{

8
gtsam/geometry/Cal3Unified.cpp
View File

@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v):
Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
/* ************************************************************************* */
Vector Cal3Unified::vector() const {
return (Vector(10) << Base::vector(), xi_).finished();
Vector10 Cal3Unified::vector() const {
Vector10 v;
v << Base::vector(), xi_;
return v;
}
/* ************************************************************************* */
@ -133,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
}
/* ************************************************************************* */
Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
return T2.vector() - vector();
}

4
gtsam/geometry/Cal3Unified.h
View File

@ -52,7 +52,7 @@ private:
public:
enum { dimension = 10 };
Vector vector() const ;
Vector10 vector() const ;
/// @name Standard Constructors
/// @{
@ -117,7 +117,7 @@ public:
Cal3Unified retract(const Vector& d) const ;
/// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3Unified& T2) const ;
Vector10 localCoordinates(const Cal3Unified& T2) const ;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)

9
gtsam/geometry/Cal3_S2.h
View File

@ -117,10 +117,9 @@ public:
}
/// vectorized form (column-wise)
Vector vector() const {
double r[] = { fx_, fy_, s_, u0_, v0_ };
Vector v(5);
std::copy(r, r + 5, v.data());
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
@ -199,7 +198,7 @@ public:
}
/// Unretraction for the calibration
Vector localCoordinates(const Cal3_S2& T2) const {
Vector5 localCoordinates(const Cal3_S2& T2) const {
return T2.vector() - vector();
}

39
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -34,6 +34,7 @@ namespace gtsam {
public:
enum { dimension = 6 };
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
/// @name Standard Constructors
@ -103,6 +104,38 @@ namespace gtsam {
/// return baseline
inline double baseline() const { return b_; }
/// vectorized form (column-wise)
Vector6 vector() const {
Vector6 v;
v << K_.vector(), b_;
return v;
}
/// @}
/// @name Manifold
/// @{
/// return DOF, dimensionality of tangent space
inline size_t dim() const {
return 6;
}
/// return DOF, dimensionality of tangent space
static size_t Dim() {
return 6;
}
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
}
/// Unretraction for the calibration
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
/// @}
/// @name Advanced Interface
/// @{
@ -119,4 +152,10 @@ namespace gtsam {
/// @}
};
// Define GTSAM traits
template<>
struct traits_x<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam

8
gtsam/geometry/EssentialMatrix.cpp
View File

@ -51,9 +51,11 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
}
/* ************************************************************************* */
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(
other.aTb_)).finished();
Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
Vector5 v;
v << aRb_.localCoordinates(other.aRb_),
aTb_.localCoordinates(other.aTb_);
return v;
}
/* ************************************************************************* */

10
gtsam/geometry/EssentialMatrix.h
View File

@ -33,8 +33,8 @@ public:
enum { dimension = 5 };
/// Static function to convert Point2 to homogeneous coordinates
static Vector Homogeneous(const Point2& p) {
return (Vector(3) << p.x(), p.y(), 1).finished();
static Vector3 Homogeneous(const Point2& p) {
return Vector3(p.x(), p.y(), 1);
}
/// @name Constructors and named constructors
@ -86,15 +86,15 @@ public:
}
/// Return the dimensionality of the tangent space
virtual size_t dim() const {
size_t dim() const {
return 5;
}
/// Retract delta to manifold
virtual EssentialMatrix retract(const Vector& xi) const;
EssentialMatrix retract(const Vector& xi) const;
/// Compute the coordinates in the tangent space
virtual Vector localCoordinates(const EssentialMatrix& other) const;
Vector5 localCoordinates(const EssentialMatrix& other) const;
/// @}

15
gtsam/geometry/Point2.h
View File

@ -184,12 +184,25 @@ public:
}
/// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); }
static inline Vector Logmap(const Point2& dp) {
return (Vector(2) << dp.x(), dp.y()).finished();
}
/// Version with Jacobian
static Vector Logmap(const Point2& dp, OptionalJacobian<2,2> H) {
CONCEPT_NOT_IMPLEMENTED;
return (Vector(2) << dp.x(), dp.y()).finished();
}
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector2& v) {
return eye(2);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector2& v) {
return eye(2);
}
/// @}
/// @name Vector Space

70
gtsam/geometry/Pose2.cpp
View File

@ -75,18 +75,18 @@ Pose2 Pose2::Expmap(const Vector& xi) {
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
Vector3 Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return (Vector(3) << t.x(), t.y(), w).finished();
return Vector3(t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return (Vector(3) << v.x(), v.y(), w).finished();
return Vector3(v.x(), v.y(), w);
}
}
@ -101,12 +101,12 @@ Pose2 Pose2::retract(const Vector& v) const {
}
/* ************************************************************************* */
Vector Pose2::localCoordinates(const Pose2& p2) const {
Vector3 Pose2::localCoordinates(const Pose2& p2) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
return Logmap(between(p2));
#else
Pose2 r = between(p2);
return (Vector(3) << r.x(), r.y(), r.theta()).finished();
return Vector3(r.x(), r.y(), r.theta());
#endif
}
@ -128,6 +128,66 @@ Matrix3 Pose2::AdjointMap() const {
return rvalue;
}
/* ************************************************************************* */
Matrix3 Pose2::adjointMap(const Vector& v) {
// See Chirikjian12book2, vol.2, pg. 36
Matrix3 ad = zeros(3,3);
ad(0,1) = -v[2];
ad(1,0) = v[2];
ad(0,2) = v[1];
ad(1,2) = -v[0];
return ad;
}
/* ************************************************************************* */
Matrix3 Pose2::dexpL(const Vector3& v) {
double alpha = v[2];
if (fabs(alpha) > 1e-5) {
// Chirikjian11book2, pg. 36
/* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26
* Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation
* In fact, Iserles 2.42 can be written as:
* \dot{g} g^{-1} = dexpR_{q}\dot{q}
* where q = A, and g = exp(A)
* and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26.
* Hence, to compute dexpL, we have to use the formula of J_r Chirikjian11book2, pg.36
*/
double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
return (Matrix(3,3) <<
sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
0, 0, 1).finished();
}
else {
// Thanks to Krunal: Apply L'Hospital rule to several times to
// compute the limits when alpha -> 0
return (Matrix(3,3) << 1,0,-0.5*v[1],
0,1, 0.5*v[0],
0,0, 1).finished();
}
}
/* ************************************************************************* */
Matrix3 Pose2::dexpInvL(const Vector3& v) {
double alpha = v[2];
if (fabs(alpha) > 1e-5) {
double alphaInv = 1/alpha;
double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
double v1 = v[0], v2 = v[1];
return (Matrix(3,3) <<
alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
0, 0, 1).finished();
}
else {
return (Matrix(3,3) << 1,0, 0.5*v[1],
0,1, -0.5*v[0],
0,0, 1).finished();
}
}
/* ************************************************************************* */
Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -AdjointMap();

25
gtsam/geometry/Pose2.h
View File

@ -141,7 +141,7 @@ public:
Pose2 retract(const Vector& v) const;
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
Vector localCoordinates(const Pose2& p2) const;
Vector3 localCoordinates(const Pose2& p2) const;
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
Vector localCoordinates(const Pose2& p2, OptionalJacobian<3,3> Hthis, OptionalJacobian<3,3> Hother) const;
@ -154,7 +154,7 @@ public:
static Pose2 Expmap(const Vector& xi);
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
static Vector Logmap(const Pose2& p);
static Vector3 Logmap(const Pose2& p);
/**
* Calculate Adjoint map
@ -166,6 +166,11 @@ public:
return AdjointMap()*xi;
}
/**
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
*/
static Matrix3 adjointMap(const Vector& v);
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
@ -174,12 +179,20 @@ public:
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix3 wedge(double vx, double vy, double w) {
return (Matrix(3,3) <<
0.,-w, vx,
w, 0., vy,
0., 0., 0.).finished();
Matrix3 m;
m << 0.,-w, vx,
w, 0., vy,
0., 0., 0.;
return m;
}
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector3& v);
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v);
/// @}
/// @name Group Action on Point2
/// @{

1
gtsam/geometry/Pose3.h
View File

@ -221,6 +221,7 @@ public:
* and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2
* Ernst Hairer, et al., Geometric Numerical Integration,
* Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
* See also Iserles05an, pg. 33, formula 2.46
*/
static Matrix6 dExpInv_exp(const Vector6 &xi);

18
gtsam/geometry/Rot2.h
View File

@ -155,7 +155,7 @@ namespace gtsam {
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
/// Returns inverse retraction
inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
/// @}
/// @name Lie Group
@ -170,8 +170,20 @@ namespace gtsam {
}
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) {
return (Vector(1) << r.theta()).finished();
static inline Vector1 Logmap(const Rot2& r) {
Vector1 v;
v << r.theta();
return v;
}
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {
return ones(1);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector& v) {
return ones(1);
}
/// @}

2
gtsam/geometry/Rot3.cpp
View File

@ -52,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
Rot3 Rot3::rodriguez(const Vector3& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);

4
gtsam/geometry/Rot3.h
View File

@ -181,7 +181,7 @@ namespace gtsam {
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& v);
static Rot3 rodriguez(const Vector3& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
@ -191,7 +191,7 @@ namespace gtsam {
* @return incremental rotation matrix
*/
static Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez((Vector(3) << wx, wy, wz).finished());}
{ return rodriguez(Vector3(wx, wy, wz));}
/// @}
/// @name Testable

8
gtsam/geometry/StereoCamera.cpp
View File

@ -91,11 +91,11 @@ namespace gtsam {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
return (Matrix(3, 3) <<
f_x*d, 0.0, -d2*f_x* P.x(),
Matrix3 m;
m << f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y()
).finished();
0.0, f_y*d, -d2*f_y* P.y();
return m;
}
}

10
gtsam/geometry/StereoCamera.h
View File

@ -92,14 +92,8 @@ public:
}
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const StereoCamera& t2) const {
return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_));
}
Pose3 between(const StereoCamera &camera,
OptionalJacobian<6,6> H1=boost::none,
OptionalJacobian<6,6> H2=boost::none) const {
return leftCamPose_.between(camera.pose(), H1, H2);
inline Vector6 localCoordinates(const StereoCamera& t2) const {
return leftCamPose_.localCoordinates(t2.leftCamPose_);
}
/// @}

2
gtsam/geometry/Unit3.cpp
View File

@ -164,7 +164,7 @@ Vector2 Unit3::localCoordinates(const Unit3& y) const {
if (std::abs(dot - 1.0) < 1e-16)
return Vector2(0, 0);
else if (std::abs(dot + 1.0) < 1e-16)
return (Vector(2) << M_PI, 0).finished();
return Vector2(M_PI, 0);
else {
// no special case
double theta = acos(dot);

35
gtsam/geometry/tests/testPose2.cpp
View File

@ -32,7 +32,7 @@ using namespace boost::assign;
using namespace gtsam;
using namespace std;
// #define SLOW_BUT_CORRECT_EXPMAP
//#define SLOW_BUT_CORRECT_EXPMAP
GTSAM_CONCEPT_TESTABLE_INST(Pose2)
GTSAM_CONCEPT_LIE_INST(Pose2)
@ -143,7 +143,7 @@ TEST(Pose2, expmap0d) {
// test case for screw motion in the plane
namespace screw {
double w=0.3;
Vector xi = (Vector(3) << 0.0, w, w);
Vector xi = (Vector(3) << 0.0, w, w).finished();
Rot2 expectedR = Rot2::fromAngle(w);
Point2 expectedT(-0.0446635, 0.29552);
Pose2 expected(expectedR, expectedT);
@ -192,6 +192,37 @@ TEST(Pose2, logmap_full) {
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
Vector3 w = (Vector(3) << 0.1, 0.27, -0.3).finished();
Vector3 w0 = (Vector(3) << 0.1, 0.27, 0.0).finished(); // alpha = 0
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3 w, const Vector3& dw) {
Vector3 y = Pose2::Logmap(Pose2::Expmap(-w) * Pose2::Expmap(w + dw));
return y;
}
TEST( Pose2, dexpL) {
Matrix actualDexpL = Pose2::dexpL(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Pose2::dexpInvL(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
// test case where alpha = 0
Matrix actualDexpL0 = Pose2::dexpL(w0);
Matrix expectedDexpL0 = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w0, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5));
Matrix actualDexpInvL0 = Pose2::dexpInvL(w0);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
}
/* ************************************************************************* */
static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
return pose.transform_to(point);

22
gtsam/geometry/tests/testRot2.cpp
View File

@ -155,6 +155,28 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */
Vector w = (Vector(1) << 0.27).finished();
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector1 testDexpL(const Vector& dw) {
Vector1 y = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw));
return y;
}
TEST( Rot2, dexpL) {
Matrix actualDexpL = Rot2::dexpL(w);
Matrix expectedDexpL = numericalDerivative11<Vector, Vector1>(
boost::function<Vector(const Vector&)>(
boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot2::dexpInvL(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;

38
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -74,7 +74,7 @@ TEST( StereoCamera, project)
/* ************************************************************************* */
static Pose3 camera1((Matrix)(Matrix(3,3) <<
static Pose3 camPose((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -82,33 +82,41 @@ static Pose3 camera1((Matrix)(Matrix(3,3) <<
Point3(0,0,6.25));
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
static StereoCamera stereoCam(Pose3(), K);
static StereoCamera stereoCam(camPose, K);
// point X Y Z in meters
static Point3 p(0, 0, 5);
static Point3 landmark(0, 0, 5);
/* ************************************************************************* */
static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
TEST( StereoCamera, Dproject_stereo_pose)
{
Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
Matrix actual; stereoCam.project(p, actual, boost::none);
CHECK(assert_equal(expected,actual,1e-7));
static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) {
return StereoCamera(pose, boost::make_shared<Cal3_S2Stereo>(K)).project(point);
}
/* ************************************************************************* */
TEST( StereoCamera, Dproject_stereo_point)
TEST( StereoCamera, Dproject)
{
Matrix expected = numericalDerivative22<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
Matrix actual; stereoCam.project(p, boost::none, actual);
CHECK(assert_equal(expected,actual,1e-8));
Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none);
CHECK(assert_equal(expected1,actual1,1e-7));
Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none);
CHECK(assert_equal(expected2,actual2,1e-7));
Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K);
Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3);
// CHECK(assert_equal(expected3,actual3,1e-8));
}
/* ************************************************************************* */
TEST( StereoCamera, backproject)
{
Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam2(Pose3(), K2);
Point3 expected(1.2, 2.3, 4.5);
StereoPoint2 stereo_point = stereoCam.project(expected);
Point3 actual = stereoCam.backproject(stereo_point);
StereoPoint2 stereo_point = stereoCam2.project(expected);
Point3 actual = stereoCam2.backproject(stereo_point);
CHECK(assert_equal(expected,actual,1e-8));
}

2
gtsam/linear/ConjugateGradientSolver.cpp
View File

@ -2,7 +2,7 @@
* ConjugateGradientSolver.cpp
*
* Created on: Jun 4, 2014
* Author: ydjian
* Author: Yong-Dian Jian
*/
#include <gtsam/linear/ConjugateGradientSolver.h>

40
gtsam/linear/ConjugateGradientSolver.h
View File

@ -9,6 +9,14 @@
* -------------------------------------------------------------------------- */
/**
* @file ConjugateGradientSolver.h
* @brief Implementation of Conjugate Gradient solver for a linear system
* @author Yong-Dian Jian
* @author Sungtae An
* @date Nov 6, 2014
**/
#pragma once
#include <gtsam/linear/IterativeSolver.h>
@ -82,9 +90,13 @@ public:
/*********************************************************************************************/
/*
* A template of linear preconditioned conjugate gradient method.
* System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v,
* rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
* System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
* leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
* Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
*
** REFERENCES:
* [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
* 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
*/
template <class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
@ -93,8 +105,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju
estimate = residual = direction = q1 = q2 = initial;
system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
system.rightPrecondition(residual, direction);/* d = S^{-1} r */
system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
system.rightPrecondition(residual, direction);/* p = L^{-T} r */
double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
@ -116,21 +128,21 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju
if ( k % iReset == 0 ) {
system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
system.rightPrecondition(residual, direction); /* d = S^{-1} r */
system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
system.rightPrecondition(residual, direction); /* p = L^{-T} r */
currentGamma = system.dot(residual, residual);
}
system.multiply(direction, q1); /* q1 = A d */
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */
system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */
system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */
system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */
system.multiply(direction, q1); /* q1 = A p */
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */
system.axpy(alpha, direction, estimate); /* estimate += alpha * p */
system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */
system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */
prevGamma = currentGamma;
currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */
currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */
beta = currentGamma / prevGamma;
system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */
system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */
system.scal(beta, direction);
system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */
system.axpy(1.0, q1, direction); /* p = q1 + beta * p */
if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
std::cout << "[PCG] k = " << k

3
gtsam/linear/GaussianFactor.h
View File

@ -133,6 +133,9 @@ namespace gtsam {
/// A'*b for Jacobian, eta for Hessian (raw memory version)
virtual void gradientAtZero(double* d) const = 0;
/// Gradient wrt a key at any values
virtual Vector gradient(Key key, const VectorValues& x) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;

18
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -254,6 +254,7 @@ namespace gtsam {
map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal() const {
map<Key,Matrix> blocks;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if (!factor) continue;
map<Key,Matrix> BD = factor->hessianBlockDiagonal();
map<Key,Matrix>::const_iterator it = BD.begin();
for(;it!=BD.end();it++) {
@ -303,6 +304,7 @@ namespace gtsam {
// Zero-out the gradient
VectorValues g;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if (!factor) continue;
VectorValues gi = factor->gradientAtZero();
g.addInPlace_(gi);
}
@ -393,15 +395,15 @@ namespace gtsam {
/* ************************************************************************* */
// x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorValues& x) const {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha, *(ei++), x);
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorValues& x) const {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha, *(ei++), x);
}
}
}
///* ************************************************************************* */
//void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {

24
gtsam/linear/HessianFactor.cpp
View File

@ -616,6 +616,30 @@ void HessianFactor::gradientAtZero(double* d) const {
}
}
/* ************************************************************************* */
Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
Factor::const_iterator i = find(key);
// Sum over G_ij*xj for all xj connecting to xi
Vector b = zero(x.at(key).size());
for (Factor::const_iterator j = begin(); j != end(); ++j) {
// Obtain Gij from the Hessian factor
// Hessian factor only stores an upper triangular matrix, so be careful when i>j
Matrix Gij;
if (i > j) {
Matrix Gji = info(j, i);
Gij = Gji.transpose();
}
else {
Gij = info(i, j);
}
// Accumulate Gij*xj to gradf
b += Gij * x.at(*j);
}
// Subtract the linear term gi
b += -linearTerm(i);
return b;
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)

6
gtsam/linear/HessianFactor.h
View File

@ -389,6 +389,12 @@ namespace gtsam {
virtual void gradientAtZero(double* d) const;
/**
* Compute the gradient at a key:
* \grad f(x_i) = \sum_j G_ij*x_j - g_i
*/
Vector gradient(Key key, const VectorValues& x) const;
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors are
* left-multiplied with their transpose to form the Hessian using the conversion constructor

9
gtsam/linear/JacobianFactor.cpp
View File

@ -575,7 +575,14 @@ VectorValues JacobianFactor::gradientAtZero() const {
/* ************************************************************************* */
void JacobianFactor::gradientAtZero(double* d) const {
//throw std::runtime_error("gradientAtZero not implemented for Jacobian factor");
throw std::runtime_error("Raw memory version of gradientAtZero not implemented for Jacobian factor");
}
/* ************************************************************************* */
Vector JacobianFactor::gradient(Key key, const VectorValues& x) const {
// TODO: optimize it for JacobianFactor without converting to a HessianFactor
HessianFactor hessian(*this);
return hessian.gradient(key, x);
}
/* ************************************************************************* */

9
gtsam/linear/JacobianFactor.h
View File

@ -230,7 +230,9 @@ namespace gtsam {
virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }
/** is noise model constrained ? */
bool isConstrained() const { return model_->isConstrained(); }
bool isConstrained() const {
return model_ && model_->isConstrained();
}
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
@ -288,9 +290,12 @@ namespace gtsam {
/// A'*b for Jacobian
VectorValues gradientAtZero() const;
/* ************************************************************************* */
/// A'*b for Jacobian (raw memory version)
virtual void gradientAtZero(double* d) const;
/// Compute the gradient wrt a key at any values
Vector gradient(Key key, const VectorValues& x) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const;

111
gtsam/linear/PCGSolver.cpp
View File

@ -2,20 +2,14 @@
* PCGSolver.cpp
*
* Created on: Feb 14, 2012
* Author: ydjian
* Author: Yong-Dian Jian
* Author: Sungtae An
*/
#include <gtsam/linear/GaussianFactorGraph.h>
//#include <gtsam/inference/FactorGraph-inst.h>
//#include <gtsam/linear/FactorGraphUtil-inl.h>
//#include <gtsam/linear/JacobianFactorGraph.h>
//#include <gtsam/linear/LSPCGSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
//#include <gtsam/linear/SuiteSparseUtil.h>
//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
//#include <gsp2/gtsam-interface-sbm.h>
//#include <ydjian/tool/ThreadSafeTimer.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <stdexcept>
@ -33,6 +27,7 @@ void PCGSolverParameters::print(ostream &os) const {
/*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) {
parameters_ = p;
preconditioner_ = createPreconditioner(p.preconditioner_);
}
@ -76,97 +71,47 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
}
/*****************************************************************************/
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
/* implement Ax, assume x and Ax are pre-allocated */
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
/* implement A^T*(A*x), assume x and AtAx are pre-allocated */
/* reset y */
Ax.setZero();
// Build a VectorValues for Vector x
VectorValues vvX = buildVectorValues(x,keyInfo_);
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
/* accumulate At A x*/
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const Matrix Ai = jf->getA(it);
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
Ax.segment(entry.colstart(), entry.dim())
+= Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim()));
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate H x */
// VectorValues form of A'Ax for multiplyHessianAdd
VectorValues vvAtAx;
/* use buffer to avoid excessive table lookups */
const size_t sz = hf->size();
vector<Vector> y;
y.reserve(sz);
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
/* initialize y to zeros */
y.push_back(zero(hf->getDim(it)));
}
// vvAtAx += 1.0 * A'Ax for each factor
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) {
/* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(*j)->second;
// xj is the input vector
const Vector xj = x.segment(entry.colstart(), entry.dim());
size_t idx = 0;
for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) {
if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj;
else y[idx] += hf->info(i, j).knownOffDiagonal() * xj;
}
}
// Make the result as Vector form
AtAx = vvAtAx.vector();
/* accumulate to r */
for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
/* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
Ax.segment(entry.colstart(), entry.dim()) += y[i];
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
}
/*****************************************************************************/
void GaussianFactorGraphSystem::getb(Vector &b) const {
/* compute rhs, assume b pre-allocated */
/* reset */
b.setZero();
// Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues
VectorValues vvb = gfg_.gradientAtZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
const Vector rhs = jf->getb();
/* accumulate At rhs */
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate g */
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
// Make the result as Vector form
b = -vvb.vector();
}
/**********************************************************************************/
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.transposeSolve(x, y); }
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const {
// For a preconditioner M = L*L^T
// Calculate y = L^{-1} x
preconditioner_.solve(x, y);
}
/**********************************************************************************/
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.solve(x, y); }
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const {
// For a preconditioner M = L*L^T
// Calculate y = L^{-T} x
preconditioner_.transposeSolve(x, y);
}
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v,

18
gtsam/linear/Preconditioner.cpp
View File

@ -2,7 +2,8 @@
* Preconditioner.cpp
*
* Created on: Jun 2, 2014
* Author: ydjian
* Author: Yong-Dian Jian
* Author: Sungtae An
*/
#include <gtsam/inference/FactorGraph-inst.h>
@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
R.triangularView<Eigen::Upper>().solveInPlace(b);
R.triangularView<Eigen::Lower>().solveInPlace(b);
dst += d;
ptr += sz;
@ -141,11 +142,9 @@ void BlockJacobiPreconditioner::build(
}
/* getting the block diagonals over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
blocks.push_back(hessian);
}
std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
blocks.push_back(hessian);
/* if necessary, allocating the memory for cacheing the factorization results */
if ( nnz > bufferSize_ ) {
@ -159,11 +158,12 @@ void BlockJacobiPreconditioner::build(
double *ptr = buffer_;
for ( size_t i = 0 ; i < n ; ++i ) {
/* use eigen to decompose Di */
const Matrix R = blocks[i].llt().matrixL().transpose();
/* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */
const Matrix L = blocks[i].llt().matrixL();
/* store the data in the buffer */
size_t sz = dims_[i]*dims_[i] ;
std::copy(R.data(), R.data() + sz, ptr);
std::copy(L.data(), L.data() + sz, ptr);
/* advance the pointer */
ptr += sz;

12
gtsam/linear/Preconditioner.h
View File

@ -2,7 +2,8 @@
* Preconditioner.h
*
* Created on: Jun 2, 2014
* Author: ydjian
* Author: Yong-Dian Jian
* Author: Sungtae An
*/
#pragma once
@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters {
};
/* PCG aims to solve the problem: A x = b by reparametrizing it as
* S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M
* L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b,
* where A \approx L L^{T}, or A \approx M
* The goal of this class is to provide a general interface to all preconditioners */
class GTSAM_EXPORT Preconditioner {
public:
@ -70,15 +72,15 @@ public:
/* Computation Interfaces */
/* implement x = S^{-1} y */
/* implement x = L^{-1} y */
virtual void solve(const Vector& y, Vector &x) const = 0;
// virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
/* implement x = S^{-T} y */
/* implement x = L^{-T} y */
virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
// /* implement x = S^{-1} S^{-T} y */
// /* implement x = L^{-1} L^{-T} y */
// virtual void fullSolve(const Vector& y, Vector &x) const = 0;
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;

26
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -449,6 +449,32 @@ TEST(HessianFactor, gradientAtZero)
EXPECT(assert_equal(expectedG, actualG));
}
/* ************************************************************************* */
TEST(HessianFactor, gradient)
{
Matrix G11 = (Matrix(1, 1) << 1).finished();
Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
Vector g1 = (Vector(1) << -7).finished();
Vector g2 = (Vector(2) << -8, -9).finished();
double f = 194;
HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
// test gradient
Vector x0 = (Vector(1) << 3.0).finished();
Vector x1 = (Vector(2) << -3.5, 7.1).finished();
VectorValues x = pair_list_of<Key, Vector>(0, x0) (1, x1);
Vector expectedGrad0 = (Vector(1) << 10.0).finished();
Vector expectedGrad1 = (Vector(2) << 4.5, 16.1).finished();
Vector grad0 = factor.gradient(0, x);
Vector grad1 = factor.gradient(1, x);
EXPECT(assert_equal(expectedGrad0, grad0));
EXPECT(assert_equal(expectedGrad1, grad1));
}
/* ************************************************************************* */
TEST(HessianFactor, hessianDiagonal)
{

5
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/navigation/MagFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieScalar.h>
#include <CppUnitLite/TestHarness.h>
@ -73,7 +72,7 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot2> //
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1
@ -95,7 +94,7 @@ TEST( MagFactor, Factors ) {
// MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector,double> //
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //

4
gtsam/nonlinear/Expression-inl.h
View File

@ -516,8 +516,8 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
/// Given df/dT, multiply in dT/dA and continue reverse AD process
// Cols is always known at compile time
template<int Rows, int Cols>
void reverseAD4(const Eigen::Matrix<double, Rows, Cols> & dFdT,
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD4(dFdT, jacobians);
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);

14
gtsam/nonlinear/Expression.h
View File

@ -206,10 +206,24 @@ private:
// with an execution trace, made up entirely of "Record" structs, see
// the FunctionalNode class in expression-inl.h
size_t size = traceSize();
// Windows does not support variable length arrays, so memory must be dynamically
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size];
#else
ExecutionTraceStorage traceStorage[size];
#endif
ExecutionTrace<T> trace;
T value(traceExecution(values, trace, traceStorage));
trace.startReverseAD1(jacobians);
#ifdef _MSC_VER
delete[] traceStorage;
#endif
return value;
}

36
gtsam/nonlinear/Values.cpp
View File

@ -112,6 +112,24 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
Vector Values::atFixed(Key j, size_t n) {
switch (n) {
case 1: return at<Vector1>(j);
case 2: return at<Vector2>(j);
case 3: return at<Vector3>(j);
case 4: return at<Vector4>(j);
case 5: return at<Vector5>(j);
case 6: return at<Vector6>(j);
case 7: return at<Vector7>(j);
case 8: return at<Vector8>(j);
case 9: return at<Vector9>(j);
default:
throw runtime_error(
"Values::at fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
const Value& Values::at(Key j) const {
// Find the item
@ -130,6 +148,24 @@ namespace gtsam {
throw ValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
void Values::insertFixed(Key j, const Vector& v, size_t n) {
switch (n) {
case 1: insert<Vector1>(j,v); break;
case 2: insert<Vector2>(j,v); break;
case 3: insert<Vector3>(j,v); break;
case 4: insert<Vector4>(j,v); break;
case 5: insert<Vector5>(j,v); break;
case 6: insert<Vector6>(j,v); break;
case 7: insert<Vector7>(j,v); break;
case 8: insert<Vector8>(j,v); break;
case 9: insert<Vector9>(j,v); break;
default:
throw runtime_error(
"Values::insert fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {

14
gtsam/nonlinear/Values.h
View File

@ -180,6 +180,13 @@ namespace gtsam {
template<typename ValueType>
const ValueType& at(Key j) const;
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
Vector atFixed(Key j, size_t n);
/// version for double
double atDouble(size_t key) const { return at<double>(key);}
/** Retrieve a variable by key \c j. This version returns a reference
* to the base Value class, and needs to be casted before use.
* @param j Retrieve the value associated with this key
@ -258,6 +265,13 @@ namespace gtsam {
template <typename ValueType>
void insert(Key j, const ValueType& val);
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
void insertFixed(Key j, const Vector& v, size_t n);
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
/// overloaded insert version that also specifies a chart
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val);

9
gtsam/nonlinear/tests/testValues.cpp
View File

@ -475,6 +475,15 @@ TEST(Values, Destructors) {
LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount);
}
/* ************************************************************************* */
TEST(Values, FixedSize) {
Values values;
Vector v(3); v << 5.0, 6.0, 7.0;
values.insertFixed(key1, v, 3);
Vector3 expected(5.0, 6.0, 7.0);
CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

6
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -453,6 +453,12 @@ public:
}
}
/// Gradient wrt a key at any values
Vector gradient(Key key, const VectorValues& x) const {
throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet");
}
};
// RegularImplicitSchurFactor

2
gtsam_unstable/CMakeLists.txt
View File

@ -3,6 +3,7 @@
set (gtsam_unstable_subdirs
base
geometry
linear
discrete
dynamics
nonlinear
@ -47,6 +48,7 @@ endforeach(subdir)
set(gtsam_unstable_srcs
${base_srcs}
${geometry_srcs}
${linear_srcs}
${discrete_srcs}
${dynamics_srcs}
${nonlinear_srcs}

6
gtsam_unstable/linear/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# Install headers
file(GLOB linear_headers "*.h")
install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear)
# Add all tests
add_subdirectory(tests)

120
gtsam_unstable/linear/LinearEquality.h Normal file
View File

@ -0,0 +1,120 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* LinearEquality.h
* @brief: LinearEquality derived from Base with constrained noise model
* @date: Nov 27, 2014
* @author: thduynguyen
*/
#pragma once
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam {
/**
* This class defines Linear constraints by inherit Base
* with the special Constrained noise model
*/
class LinearEquality: public JacobianFactor {
public:
typedef LinearEquality This; ///< Typedef to this class
typedef JacobianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
private:
Key dualKey_;
public:
/** default constructor for I/O */
LinearEquality() :
Base() {
}
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
explicit LinearEquality(const HessianFactor& hf) {
throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
}
/** Construct unary factor */
LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) :
Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) {
}
/** Construct binary factor */
LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
const Vector& b, Key dualKey) :
Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_(
dualKey) {
}
/** Construct ternary factor */
LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A3, const Vector& b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_(
dualKey) {
}
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) :
Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) {
}
/** Virtual destructor */
virtual ~LinearEquality() {
}
/** equals */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
return Base::equals(lf, tol);
}
/** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const {
Base::print(s, formatter);
}
/** Clone this LinearEquality */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<LinearEquality>(*this));
}
/// dual key
Key dualKey() const { return dualKey_; }
/// for active set method: equality constraints are always active
bool active() const { return true; }
/** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const {
return unweighted_error(c);
}
/** Special error for constraints.
* I think it should be zero, as this function is meant for objective cost.
* But the name "error" can be misleading.
* TODO: confirm with Frank!! */
virtual double error(const VectorValues& c) const {
return 0.0;
}
};
// LinearEquality
}// gtsam

32
gtsam_unstable/linear/LinearEqualityFactorGraph.h Normal file
View File

@ -0,0 +1,32 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/*
* LinearEqualityFactorGraph.h
* @brief: Factor graph of all LinearEquality factors
* @date: Dec 8, 2014
* @author: thduynguyen
*/
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam_unstable/linear/LinearEquality.h>
namespace gtsam {
class LinearEqualityFactorGraph : public FactorGraph<LinearEquality> {
public:
typedef boost::shared_ptr<LinearEqualityFactorGraph> shared_ptr;
};
} // namespace gtsam

143
gtsam_unstable/linear/LinearInequality.h Normal file
View File

@ -0,0 +1,143 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* LinearInequality.h
* @brief: LinearInequality derived from Base with constrained noise model
* @date: Nov 27, 2014
* @author: thduynguyen
*/
#pragma once
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam {
typedef Eigen::RowVectorXd RowVector;
/**
* This class defines Linear constraints by inherit Base
* with the special Constrained noise model
*/
class LinearInequality: public JacobianFactor {
public:
typedef LinearInequality This; ///< Typedef to this class
typedef JacobianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
private:
Key dualKey_;
bool active_;
public:
/** default constructor for I/O */
LinearInequality() :
Base(), active_(true) {
}
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
explicit LinearInequality(const HessianFactor& hf) {
throw std::runtime_error(
"Cannot convert HessianFactor to LinearInequality");
}
/** Construct unary factor */
LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
dualKey), active_(true) {
}
/** Construct binary factor */
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b,
Key dualKey) :
Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
dualKey), active_(true) {
}
/** Construct ternary factor */
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
const RowVector& A3, double b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(),
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
}
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
LinearInequality(const TERMS& terms, double b, Key dualKey) :
Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
dualKey), active_(true) {
}
/** Virtual destructor */
virtual ~LinearInequality() {
}
/** equals */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
return Base::equals(lf, tol);
}
/** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const {
if (active())
Base::print(s + " Active", formatter);
else
Base::print(s + " Inactive", formatter);
}
/** Clone this LinearInequality */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<LinearInequality>(*this));
}
/// dual key
Key dualKey() const { return dualKey_; }
/// return true if this constraint is active
bool active() const { return active_; }
/// Make this inequality constraint active
void activate() { active_ = true; }
/// Make this inequality constraint inactive
void inactivate() { active_ = false; }
/** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const {
return unweighted_error(c);
}
/** Special error for single-valued inequality constraints. */
virtual double error(const VectorValues& c) const {
return error_vector(c)[0];
}
/** dot product of row s with the corresponding vector in p */
double dotProductRow(const VectorValues& p) const {
double aTp = 0.0;
for (const_iterator xj = begin(); xj != end(); ++xj) {
Vector pj = p.at(*xj);
Vector aj = getA(xj).transpose();
aTp += aj.dot(pj);
}
return aTp;
}
};
// LinearInequality
}// gtsam

47
gtsam_unstable/linear/LinearInequalityFactorGraph.h Normal file
View File

@ -0,0 +1,47 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* LinearInequalityFactorGraph.h
* @brief: Factor graph of all LinearInequality factors
* @date: Dec 8, 2014
* @author: thduynguyen
*/
#pragma once
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/LinearInequality.h>
namespace gtsam {
class LinearInequalityFactorGraph: public FactorGraph<LinearInequality> {
private:
typedef FactorGraph<LinearInequality> Base;
public:
typedef boost::shared_ptr<LinearInequalityFactorGraph> shared_ptr;
/** print */
void print(const std::string& str, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
Base::print(str, keyFormatter);
}
/** equals */
bool equals(const LinearInequalityFactorGraph& other,
double tol = 1e-9) const {
return Base::equals(other, tol);
}
};
} // namespace gtsam

58
gtsam_unstable/linear/QP.h Normal file
View File

@ -0,0 +1,58 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/*
* QP.h
* @brief: Factor graphs of a Quadratic Programming problem
* @date: Dec 8, 2014
* @author: thduynguyen
*/
#pragma once
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/linear/LinearInequalityFactorGraph.h>
namespace gtsam {
/**
* struct contains factor graphs of a Quadratic Programming problem
*/
struct QP {
GaussianFactorGraph cost; //!< Quadratic cost factors
LinearEqualityFactorGraph equalities; //!< linear equality constraints
LinearInequalityFactorGraph inequalities; //!< linear inequality constraints
/** default constructor */
QP() :
cost(), equalities(), inequalities() {
}
/** constructor */
QP(const GaussianFactorGraph& _cost,
const LinearEqualityFactorGraph& _linearEqualities,
const LinearInequalityFactorGraph& _linearInequalities) :
cost(_cost), equalities(_linearEqualities), inequalities(
_linearInequalities) {
}
/** print */
void print(const std::string& s = "") {
std::cout << s << std::endl;
cost.print("Quadratic cost factors: ");
equalities.print("Linear equality factors: ");
inequalities.print("Linear inequality factors: ");
}
};
} // namespace gtsam

252
gtsam_unstable/linear/QPSolver.cpp Normal file
View File

@ -0,0 +1,252 @@
/*
* QPSolver.cpp
* @brief:
* @date: Apr 15, 2014
* @author: thduynguyen
*/
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <boost/range/adaptor/map.hpp>
using namespace std;
namespace gtsam {
//******************************************************************************
QPSolver::QPSolver(const QP& qp) : qp_(qp) {
baseGraph_ = qp_.cost;
baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end());
costVariableIndex_ = VariableIndex(qp_.cost);
equalityVariableIndex_ = VariableIndex(qp_.equalities);
inequalityVariableIndex_ = VariableIndex(qp_.inequalities);
constrainedKeys_ = qp_.equalities.keys();
constrainedKeys_.merge(qp_.inequalities.keys());
}
//******************************************************************************
VectorValues QPSolver::solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
if (factor->active())
workingGraph.push_back(factor);
}
return workingGraph.optimize();
}
//******************************************************************************
JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the dual key
std::vector<std::pair<Key, Matrix> > Aterms = collectDualJacobians
< LinearEquality > (key, qp_.equalities, equalityVariableIndex_);
std::vector<std::pair<Key, Matrix> > AtermsInequalities = collectDualJacobians
< LinearInequality > (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end());
// Collect the gradients of unconstrained cost factors to the b vector
if (Aterms.size() > 0) {
Vector b = zero(delta.at(key).size());
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
b += factor->gradient(key, delta);
}
}
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows()));
}
else {
return boost::make_shared<JacobianFactor>();
}
}
//******************************************************************************
GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
BOOST_FOREACH(Key key, constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta);
if (!dualFactor->empty())
dualGraph->push_back(dualFactor);
}
return dualGraph;
}
//******************************************************************************
int QPSolver::identifyLeavingConstraint(
const LinearInequalityFactorGraph& workingSet,
const VectorValues& lambdas) const {
int worstFactorIx = -1;
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
// inactive or a good inequality constraint, so we don't care!
double maxLambda = 0.0;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
if (factor->active()) {
double lambda = lambdas.at(factor->dualKey())[0];
if (lambda > maxLambda) {
worstFactorIx = factorIx;
maxLambda = lambda;
}
}
}
return worstFactorIx;
}
//******************************************************************************
/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints
* If some inactive inequality constraints complain about the full step (alpha = 1),
* we have to adjust alpha to stay within the inequality constraints' feasible regions.
*
* For each inactive inequality j:
* - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints
* - We want: aj'*(xk + alpha*p) - bj <= 0
* - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
* it's good!
* - We only care when aj'*p > 0. In this case, we need to choose alpha so that
* aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p)
* We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
*
* We want the minimum of all those alphas among all inactive inequality.
*/
boost::tuple<double, int> QPSolver::computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const {
static bool debug = false;
double minAlpha = 1.0;
int closestFactorIx = -1;
for(size_t factorIx = 0; factorIx<workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
double b = factor->getb()[0];
// only check inactive factors
if (!factor->active()) {
// Compute a'*p
double aTp = factor->dotProductRow(p);
// Check if a'*p >0. Don't care if it's not.
if (aTp <= 0)
continue;
// Compute a'*xk
double aTx = factor->dotProductRow(xk);
// alpha = (b - a'*xk) / (a'*p)
double alpha = (b - aTx) / aTp;
if (debug)
cout << "alpha: " << alpha << endl;
// We want the minimum of all those max alphas
if (alpha < minAlpha) {
closestFactorIx = factorIx;
minAlpha = alpha;
}
}
}
return boost::make_tuple(minAlpha, closestFactorIx);
}
//******************************************************************************
QPState QPSolver::iterate(const QPState& state) const {
static bool debug = false;
// Solve with the current working set
VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet);
if (debug)
newValues.print("New solution:");
// If we CAN'T move further
if (newValues.equals(state.values, 1e-5)) {
// Compute lambda from the dual graph
if (debug)
cout << "Building dual graph..." << endl;
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues);
if (debug)
dualGraph->print("Dual graph: ");
VectorValues duals = dualGraph->optimize();
if (debug)
duals.print("Duals :");
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
if (debug)
cout << "leavingFactor: " << leavingFactor << endl;
// If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) {
return QPState(newValues, duals, state.workingSet, true);
}
else {
// Inactivate the leaving constraint
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false);
}
}
else {
// If we CAN make some progress
// Adapt stepsize if some inactive constraints complain about this move
double alpha;
int factorIx;
VectorValues p = newValues - state.values;
boost::tie(alpha, factorIx) = //
computeStepSize(state.workingSet, state.values, p);
if (debug)
cout << "alpha, factorIx: " << alpha << " " << factorIx << " "
<< endl;
// also add to the working set the one that complains the most
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
if (factorIx >= 0)
newWorkingSet.at(factorIx)->activate();
// step!
newValues = state.values + alpha * p;
return QPState(newValues, state.duals, newWorkingSet, false);
}
}
//******************************************************************************
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues) const {
LinearInequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
double error = workingFactor->error(initialValues);
if (fabs(error)>1e-7){
workingFactor->inactivate();
} else {
workingFactor->activate();
}
workingSet.push_back(workingFactor);
}
return workingSet;
}
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initialValues) const {
// Initialize workingSet from the feasible initialValues
LinearInequalityFactorGraph workingSet =
identifyActiveConstraints(qp_.inequalities, initialValues);
QPState state(initialValues, VectorValues(), workingSet, false);
/// main loop of the solver
while (!state.converged) {
state = iterate(state);
}
return make_pair(state.values, state.duals);
}
} /* namespace gtsam */

188
gtsam_unstable/linear/QPSolver.h Normal file
View File

@ -0,0 +1,188 @@
/*
* QPSolver.h
* @brief: A quadratic programming solver implements the active set method
* @date: Apr 15, 2014
* @author: thduynguyen
*/
#pragma once
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/QP.h>
#include <vector>
#include <set>
namespace gtsam {
/// This struct holds the state of QPSolver at each iteration
struct QPState {
VectorValues values;
VectorValues duals;
LinearInequalityFactorGraph workingSet;
bool converged;
/// default constructor
QPState() :
values(), duals(), workingSet(), converged(false) {
}
/// constructor with initial values
QPState(const VectorValues& initialValues, const VectorValues& initialDuals,
const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) :
values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
_converged) {
}
};
/**
* This class implements the active set method to solve quadratic programming problems
* encoded in a GaussianFactorGraph with special mixed constrained noise models, in which
* a negative sigma denotes an inequality <=0 constraint,
* a zero sigma denotes an equality =0 constraint,
* and a positive sigma denotes a normal Gaussian noise model.
*/
class QPSolver {
const QP& qp_; //!< factor graphs of the QP problem, can't be modified!
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process.
VariableIndex costVariableIndex_, equalityVariableIndex_,
inequalityVariableIndex_;
FastSet<Key> constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
public:
/// Constructor
QPSolver(const QP& qp);
/// Find solution with the current working set
VectorValues solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const;
/// @name Build the dual graph
/// @{
/// Collect the Jacobian terms for a dual factor
template<typename FACTOR>
std::vector<std::pair<Key, Matrix> > collectDualJacobians(Key key,
const FactorGraph<FACTOR>& graph,
const VariableIndex& variableIndex) const {
std::vector<std::pair<Key, Matrix> > Aterms;
if (variableIndex.find(key) != variableIndex.end()) {
BOOST_FOREACH(size_t factorIx, variableIndex[key]){
typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active()) continue;
Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
}
}
return Aterms;
}
/// Create a dual factor
JacobianFactor::shared_ptr createDualFactor(Key key,
const LinearInequalityFactorGraph& workingSet,
const VectorValues& delta) const;
/**
* Build the dual graph to solve for the Lagrange multipliers.
*
* The Lagrangian function is:
* L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X),
* where the unconstrained part is
* f(X) = 0.5*X'*G*X - X'*g + 0.5*f0
* and the linear equality constraints are
* c1(X), c2(X), ..., cm(X)
*
* Take the derivative of L wrt X at the solution and set it to 0, we have
* \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*)
*
* For each set of rows of (*) corresponding to a variable xi involving in some constraints
* we have:
* \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi
* \grad c_k(xi) = \frac{\partial c_k}{\partial xi}'
*
* Note: If xi does not involve in any constraint, we have the trivial condition
* \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables.
*
* So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0
* on the constraints' lambda multipliers, as follows:
* - The jacobian term A_k for each lambda_k is \grad c_k(xi)
* - The constant term b is \grad f(xi), which can be computed from all unconstrained
* Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi
*/
GaussianFactorGraph::shared_ptr buildDualGraph(
const LinearInequalityFactorGraph& workingSet,
const VectorValues& delta) const;
/// @}
/**
* The goal of this function is to find currently active inequality constraints
* that violate the condition to be active. The one that violates the condition
* the most will be removed from the active set. See Nocedal06book, pg 469-471
*
* Find the BAD active inequality that pulls x strongest to the wrong direction
* of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0)
*
* For active inequality constraints (those that are enforced as equality constraints
* in the current working set), we want lambda < 0.
* This is because:
* - From the Lagrangian L = f - lambda*c, we know that the constraint force
* is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay
* on the constraint surface, the constraint force has to balance out with
* other unconstrained forces that are pulling x towards the unconstrained
* minimum point. The other unconstrained forces are pulling x toward (-\grad f),
* hence the constraint force has to be exactly \grad f, so that the total
* force is 0.
* - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0),
* while we are solving for - (<=0) constraint.
* - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction
* i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied.
* That means we want lambda < 0.
* - This is because when the constrained force pulls x towards the infeasible region (+),
* the unconstrained force is pulling x towards the opposite direction into
* the feasible region (again because the total force has to be 0 to make x stay still)
* So we can drop this constraint to have a lower error but feasible solution.
*
* In short, active inequality constraints with lambda > 0 are BAD, because they
* violate the condition to be active.
*
* And we want to remove the worst one with the largest lambda from the active set.
*
*/
int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet,
const VectorValues& lambdas) const;
/**
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
*
* @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex)
* is the constraint that has minimum alpha, or (-1,-1) if alpha = 1.
* This constraint will be added to the working set and become active
* in the next iteration
*/
boost::tuple<double, int> computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const;
/** Iterate 1 step, return a new state with a new workingSet and values */
QPState iterate(const QPState& state) const;
/**
* Identify active constraints based on initial values.
*/
LinearInequalityFactorGraph identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues) const;
/** Optimize with a provided initial values
* For this version, it is the responsibility of the caller to provide
* a feasible initial value.
* @return a pair of <primal, dual> solutions
*/
std::pair<VectorValues, VectorValues> optimize(
const VectorValues& initialValues) const;
};
} /* namespace gtsam */

1
gtsam_unstable/linear/tests/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
gtsamAddTestsGlob(linear_unstable "test*.cpp" "" "gtsam_unstable")

237
gtsam_unstable/linear/tests/testLinearEquality.cpp Normal file
View File

@ -0,0 +1,237 @@
/* ----------------------------------------------------------------------------
* 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 testLinearEquality.cpp
* @brief Unit tests for LinearEquality
* @author thduynguyen
**/
#include <gtsam_unstable/linear/LinearEquality.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
namespace {
namespace simple {
// Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
(make_pair(5, Matrix3::Identity()))
(make_pair(10, 2*Matrix3::Identity()))
(make_pair(15, 3*Matrix3::Identity()));
// RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.).finished();
const SharedDiagonal noise = noiseModel::Constrained::All(3);
}
}
/* ************************************************************************* */
TEST(LinearEquality, constructors_and_accessors)
{
using namespace simple;
// Test for using different numbers of terms
{
// One term constructor
LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
LinearEquality actual(terms[0].first, terms[0].second, b, 0);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
EXPECT(assert_equal(b, expected.getb()));
EXPECT(assert_equal(b, actual.getb()));
EXPECT(assert_equal(*noise, *actual.get_model()));
}
{
// Two term constructor
LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
LinearEquality actual(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, b, 0);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
EXPECT(assert_equal(b, expected.getb()));
EXPECT(assert_equal(b, actual.getb()));
EXPECT(assert_equal(*noise, *actual.get_model()));
}
{
// Three term constructor
LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
LinearEquality actual(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
EXPECT(assert_equal(b, expected.getb()));
EXPECT(assert_equal(b, actual.getb()));
EXPECT(assert_equal(*noise, *actual.get_model()));
}
}
/* ************************************************************************* */
TEST(LinearEquality, Hessian_conversion) {
HessianFactor hessian(0, (Matrix(4,4) <<
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25).finished(),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725);
try {
LinearEquality actual(hessian);
EXPECT(false);
}
catch (const std::runtime_error& exception) {
EXPECT(true);
}
}
/* ************************************************************************* */
TEST(LinearEquality, error)
{
LinearEquality factor(simple::terms, simple::b, 0);
VectorValues values;
values.insert(5, Vector::Constant(3, 1.0));
values.insert(10, Vector::Constant(3, 0.5));
values.insert(15, Vector::Constant(3, 1.0/3.0));
Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0;
Vector actual_unwhitened = factor.unweighted_error(values);
EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
// whitened is meaningless in constraints
Vector expected_whitened(3); expected_whitened = expected_unwhitened;
Vector actual_whitened = factor.error_vector(values);
EXPECT(assert_equal(expected_whitened, actual_whitened));
double expected_error = 0.0;
double actual_error = factor.error(values);
DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
}
/* ************************************************************************* */
TEST(LinearEquality, matrices_NULL)
{
// Make sure everything works with NULL noise model
LinearEquality factor(simple::terms, simple::b, 0);
Matrix AExpected(3, 9);
AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
Vector rhsExpected = simple::b;
Matrix augmentedJacobianExpected(3, 10);
augmentedJacobianExpected << AExpected, rhsExpected;
// Whitened Jacobian
EXPECT(assert_equal(AExpected, factor.jacobian().first));
EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));
// Unwhitened Jacobian
EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first));
EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
}
/* ************************************************************************* */
TEST(LinearEquality, matrices)
{
// And now witgh a non-unit noise model
LinearEquality factor(simple::terms, simple::b, 0);
Matrix jacobianExpected(3, 9);
jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
Vector rhsExpected = simple::b;
Matrix augmentedJacobianExpected(3, 10);
augmentedJacobianExpected << jacobianExpected, rhsExpected;
Matrix augmentedHessianExpected =
augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
* simple::noise->R() * augmentedJacobianExpected;
// Whitened Jacobian
EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));
// Unwhitened Jacobian
EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
}
/* ************************************************************************* */
TEST(LinearEquality, operators )
{
Matrix I = eye(2);
Vector b = (Vector(2) << 0.2,-0.1).finished();
LinearEquality lf(1, -I, 2, I, b, 0);
VectorValues c;
c.insert(1, (Vector(2) << 10.,20.).finished());
c.insert(2, (Vector(2) << 30.,60.).finished());
// test A*x
Vector expectedE = (Vector(2) << 20.,40.).finished();
Vector actualE = lf * c;
EXPECT(assert_equal(expectedE, actualE));
// test A^e
VectorValues expectedX;
expectedX.insert(1, (Vector(2) << -20.,-40.).finished());
expectedX.insert(2, (Vector(2) << 20., 40.).finished());
VectorValues actualX = VectorValues::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, actualE, actualX);
EXPECT(assert_equal(expectedX, actualX));
// test gradient at zero
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
VectorValues expectedG;
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
VectorValues actualG = lf.gradientAtZero();
EXPECT(assert_equal(expectedG, actualG));
}
/* ************************************************************************* */
TEST(LinearEquality, default_error )
{
LinearEquality f;
double actual = f.error(VectorValues());
DOUBLES_EQUAL(0.0, actual, 1e-15);
}
//* ************************************************************************* */
TEST(LinearEquality, empty )
{
// create an empty factor
LinearEquality f;
EXPECT(f.empty());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

311
gtsam_unstable/linear/tests/testQPSolver.cpp Normal file
View File

@ -0,0 +1,311 @@
/* ----------------------------------------------------------------------------
* 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 testQPSolver.cpp
* @brief Test simple QP solver for a linear inequality constraint
* @date Apr 10, 2014
* @author Duy-Nguyen Ta
*/
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
const Matrix One = ones(1,1);
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
QP createTestCase() {
QP qp;
// Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
2.0 * ones(1, 1), zero(1), 10.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2
return qp;
}
TEST(QPSolver, TestCase) {
VectorValues values;
double x1 = 5, x2 = 7;
values.insert(X(1), x1 * ones(1, 1));
values.insert(X(2), x2 * ones(1, 1));
QP qp = createTestCase();
DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9);
DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9);
}
TEST(QPSolver, constraintsAux) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues lambdas;
lambdas.insert(0, (Vector(1) << -0.5).finished());
lambdas.insert(1, (Vector(1) << 0.0).finished());
lambdas.insert(2, (Vector(1) << 0.3).finished());
lambdas.insert(3, (Vector(1) << 0.1).finished());
int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas);
LONGS_EQUAL(2, factorIx);
VectorValues lambdas2;
lambdas2.insert(0, (Vector(1) << -0.5).finished());
lambdas2.insert(1, (Vector(1) << 0.0).finished());
lambdas2.insert(2, (Vector(1) << -0.3).finished());
lambdas2.insert(3, (Vector(1) << -0.1).finished());
int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2);
LONGS_EQUAL(-1, factorIx2);
}
/* ************************************************************************* */
// Create a simple test graph with one equality constraint
QP createEqualityConstrainedTest() {
QP qp;
// Objective functions x1^2 + x2^2
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1),
2.0 * ones(1, 1), zero(1), 0.0));
// Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
Matrix A1 = (Matrix(1, 1) << 1).finished();
Matrix A2 = (Matrix(1, 1) << 1).finished();
Vector b = -(Vector(1) << 1).finished();
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0));
return qp;
}
TEST(QPSolver, dual) {
QP qp = createEqualityConstrainedTest();
// Initials values
VectorValues initialValues;
initialValues.insert(X(1), ones(1));
initialValues.insert(X(2), ones(1));
QPSolver solver(qp);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(
qp.inequalities, initialValues);
VectorValues dual = dualGraph->optimize();
VectorValues expectedDual;
expectedDual.insert(0, (Vector(1) << 2.0).finished());
CHECK(assert_equal(expectedDual, dual, 1e-10));
}
/* ************************************************************************* */
TEST(QPSolver, indentifyActiveConstraints) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues currentSolution;
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active()); // inactive
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 0.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 0.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
/* ************************************************************************* */
TEST(QPSolver, iterate) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues currentSolution;
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished());
expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished());
expectedDuals[0].insert(1, (Vector(1) << 3).finished());
expectedDuals[0].insert(2, (Vector(1) << 0).finished());
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished());
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished());
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPState state(currentSolution, VectorValues(), workingSet, false);
int it = 0;
while (!state.converged) {
state = solver.iterate(state);
// These checks will fail because the expected solutions obtained from
// Forst10book do not follow exactly what we implemented from Nocedal06book.
// Specifically, we do not re-identify active constraints and
// do not recompute dual variables after every step!!!
// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
it++;
}
CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10));
}
/* ************************************************************************* */
TEST(QPSolver, optimizeForst10book_pg171Ex5) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), zero(1));
initialValues.insert(X(2), zero(1));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.5).finished());
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
/* ************************************************************************* */
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
QP createTestMatlabQPEx() {
QP qp;
// Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1),
2.0 * ones(1, 1), 6 * ones(1), 1000.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2
qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3
qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0
return qp;
}
TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), zero(1));
initialValues.insert(X(2), zero(1));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
// Create test graph as in Nocedal06book, Ex 16.4, pg. 475
QP createTestNocedal06bookEx16_4() {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1)));
qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1)));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1));
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2));
qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3));
qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4));
return qp;
}
TEST(QPSolver, optimizeNocedal06bookEx16_4) {
QP qp = createTestNocedal06bookEx16_4();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), (Vector(1) << 2.0).finished());
initialValues.insert(X(2), zero(1));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4).finished());
expectedSolution.insert(X(2), (Vector(1) << 1.7).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
TEST(QPSolver, failedSubproblem) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2)));
qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0));
VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
VectorValues initialValues;
initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished());
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
// graph.print("Graph: ");
// solution.print("Solution: ");
CHECK(assert_equal(expected, solution, 1e-7));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -32,6 +32,8 @@ namespace gtsam {
template<class T>
class ExpressionFactor: public NoiseModelFactor {
protected:
T measurement_; ///< the measurement to be compared with the expression
Expression<T> expression_; ///< the expression that is AD enabled
FastVector<int> dims_; ///< dimensions of the Jacobian matrices

3
gtsam_unstable/nonlinear/expressions.h
View File

@ -13,8 +13,7 @@
namespace gtsam {
// Generics
template<class T>
template<typename T>
Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
return Expression<T>(t1, &T::between, t2);
}

11
gtsam_unstable/slam/expressions.h
View File

@ -20,10 +20,7 @@ typedef Expression<Rot2> Rot2_;
typedef Expression<Pose2> Pose2_;
Point2_ transform_to(const Pose2_& x, const Point2_& p) {
Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1,
OptionalJacobian<2, 2> H2) const = &Pose2::transform_to;
return Point2_(x, transform, p);
return Point2_(x, &Pose2::transform_to, p);
}
// 3D Geometry
@ -33,11 +30,7 @@ typedef Expression<Rot3> Rot3_;
typedef Expression<Pose3> Pose3_;
Point3_ transform_to(const Pose3_& x, const Point3_& p) {
Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose,
OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to;
return Point3_(x, transform, p);
return Point3_(x, &Pose3::transform_to, p);
}
// Projection

4
matlab/gtsam_examples/IMUKittiExampleGPS.m
View File

@ -33,7 +33,7 @@ GPSskip = 10; % Skip this many GPS measurements each time
%% Get initial conditions for the estimated trajectory
currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@ -72,7 +72,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, currentBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
else
t_previous = GPS_data(measurementIndex-1, 1).Time;

4
matlab/gtsam_examples/MonocularVOExample.m
View File

@ -40,7 +40,7 @@ end
%% Create initial estimate
initial = Values;
trueE = EssentialMatrix(aRb,Sphere2(aTb));
trueE = EssentialMatrix(aRb,Unit3(aTb));
initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]');
initial.insert(1, initialE);
@ -52,5 +52,5 @@ result = optimizer.optimize();
%% Print result (as essentialMatrix) and error
error = graph.error(result)
E = result.at(1)
E = result.atEssentialMatrix(1)

18
matlab/gtsam_tests/testPriorFactor.m Normal file
View File

@ -0,0 +1,18 @@
% test wrapping of Values
import gtsam.*;
values = Values;
key = 5;
priorPose3 = Pose3;
model = noiseModel.Unit.Create(6);
factor = PriorFactorPose3(key, priorPose3, model);
values.insert(key, priorPose3);
EXPECT('error', factor.error(values) == 0);
key = 3;
priorVector = [0,0,0]';
model = noiseModel.Unit.Create(3);
factor = PriorFactorVector(key, priorVector, model);
values.insert(key, priorVector);
EXPECT('error', factor.error(values) == 0);

3
matlab/gtsam_tests/test_gtsam.m
View File

@ -1,5 +1,8 @@
% Test runner script - runs each test
% display 'Starting: testPriorFactor'
% testPriorFactor
display 'Starting: testValues'
testValues

12
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -57,7 +57,7 @@ isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!)
currentVelocityGlobal = [10;0;0]; % (This is slightly wrong!) Zhaoyang: Fixed
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
@ -101,7 +101,7 @@ axis equal;
for i=1:size(trajectory)-1
xKey = symbol('x',i);
pose = trajectory.at(xKey); % GT pose
pose = trajectory.atPose3(xKey); % GT pose
pose_t = pose.translation(); % GT pose-translation
if exist('h_cursor','var')
@ -165,13 +165,13 @@ for i=1:size(trajectory)-1
if i > 1
xKey_prev = symbol('x',i-1);
pose_prev = trajectory.at(xKey_prev);
pose_prev = trajectory.atPose3(xKey_prev);
step = pose_prev.between(pose);
% insert estimate for current pose with some normal noise on
% translation
initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
initial.insert(xKey,result.atPose3(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
% visual measurements
if measurements.size > 0 && use_camera
@ -181,12 +181,12 @@ for i=1:size(trajectory)-1
zKey = measurementKeys.at(zz);
lKey = symbol('l',symbolIndex(zKey));
fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ...
fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ...
z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
% only add landmark to values if doesn't exist yet
if ~result.exists(lKey)
noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
initial.insert(lKey, noisy_landmark);
% and add a prior since its position is known

14
matlab/unstable_examples/IMUKittiExampleAdvanced.m
View File

@ -7,7 +7,7 @@ disp('Example of application of ISAM2 for visual-inertial navigation on the KITT
%% Read metadata and compute relative sensor pose transforms
% IMU metadata
disp('-- Reading sensor metadata')
IMU_metadata = importdata('KittiEquivBiasedImu_metadata.txt');
IMU_metadata = importdata(findExampleDataFile('KittiEquivBiasedImu_metadata.txt'));
IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2);
IMUinBody = Pose3.Expmap([IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz;
IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]);
@ -16,14 +16,14 @@ if ~IMUinBody.equals(Pose3, 1e-5)
end
% VO metadata
VO_metadata = importdata('KittiRelativePose_metadata.txt');
VO_metadata = importdata(findExampleDataFile('KittiRelativePose_metadata.txt'));
VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2);
VOinBody = Pose3.Expmap([VO_metadata.BodyPtx; VO_metadata.BodyPty; VO_metadata.BodyPtz;
VO_metadata.BodyPrx; VO_metadata.BodyPry; VO_metadata.BodyPrz; ]);
VOinIMU = IMUinBody.inverse().compose(VOinBody);
% GPS metadata
GPS_metadata = importdata('KittiGps_metadata.txt');
GPS_metadata = importdata(findExampleDataFile('KittiGps_metadata.txt'));
GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2);
GPSinBody = Pose3.Expmap([GPS_metadata.BodyPtx; GPS_metadata.BodyPty; GPS_metadata.BodyPtz;
GPS_metadata.BodyPrx; GPS_metadata.BodyPry; GPS_metadata.BodyPrz; ]);
@ -32,7 +32,7 @@ GPSinIMU = IMUinBody.inverse().compose(GPSinBody);
%% Read data and change coordinate frame of GPS and VO measurements to IMU frame
disp('-- Reading sensor data from file')
% IMU data
IMU_data = importdata('KittiEquivBiasedImu.txt');
IMU_data = importdata(findExampleDataFile('KittiEquivBiasedImu.txt'));
IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2);
imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_data.accelZ]' [IMU_data.omegaX]' [IMU_data.omegaY]' [IMU_data.omegaZ]' ], 2), 'UniformOutput', false);
[IMU_data.acc_omega] = deal(imum{:});
@ -40,7 +40,7 @@ imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_da
clear imum
% VO data
VO_data = importdata('KittiRelativePose.txt');
VO_data = importdata(findExampleDataFile('KittiRelativePose.txt'));
VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2);
% Merge relative pose fields and convert to Pose3
logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ];
@ -71,7 +71,7 @@ clear logposes relposes
%% Get initial conditions for the estimated trajectory
% % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
currentPoseGlobal = Pose3;
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@ -120,7 +120,7 @@ for measurementIndex = 1:length(timestamps)
newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, currentBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
else
t_previous = timestamps(measurementIndex-1, 1);

4
matlab/unstable_examples/SmartRangeFactorExample.m
View File

@ -82,8 +82,8 @@ for ind_pose = 2:7
key_prev = poseKey(ind_pose-1);
key_curr = poseKey(ind_pose);
prev_pose = smartValues.at(key_prev);
curr_pose = smartValues.at(key_curr);
prev_pose = smartValues.atPose2(key_prev);
curr_pose = smartValues.atPose2(key_curr);
GTDeltaPose = prev_pose.between(curr_pose);
noisePose = Pose2([t_noise*rand; t_noise*rand; r_noise*rand]);
NoisyDeltaPose = GTDeltaPose.compose(noisePose);

24
matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
View File

@ -96,17 +96,17 @@ for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
initial.insert(i,result.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
initial.insert(i,result.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -137,10 +137,10 @@ for i=1:20
hold on;
%% plot results
result_camera_transform = result.at(transformKey);
result_camera_transform = result.atPose3(transformKey);
for j=1:i
gtsam.plotPose3(result.at(j),[],0.5);
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
gtsam.plotPose3(result.atPose3(j),[],0.5);
gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
@ -153,12 +153,12 @@ for i=1:20
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
plotPoint3(result.atPoint3(l),'g');
end
ty = result.at(transformKey).translation().y();
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
ty = result.atPose3(transformKey).translation().y();
fx = result.atCal3_S2(calibrationKey).fx();
fy = result.atCal3_S2(calibrationKey).fy();
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
text(1,5,3,sprintf('fx(900): %.0f',fx));
text(1,5,1,sprintf('fy(900): %.0f',fy));
@ -180,10 +180,10 @@ end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(transformKey)
result.atPose3(transformKey)
disp('Calibration after optimization');
result.at(calibrationKey)
result.atCal3_S2(calibrationKey)

4
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -89,7 +89,7 @@ isam = ISAM2(isamParams);
currentIMUPoseGlobal = Pose3();
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [1;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
@ -127,7 +127,7 @@ for i=1:steps
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
% velocity and bias evolution
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
result = initial;

12
matlab/unstable_examples/TransformProjectionFactorExample.m
View File

@ -61,17 +61,17 @@ cheirality_exception_count = 0;
for i=1:20
if i > 1
if i < 11
initial.insert(i,initial.at(i-1).compose(move_forward));
initial.insert(i,initial.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,initial.at(i-1).compose(move_circle));
initial.insert(i,initial.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -93,14 +93,14 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
%% camera plotting
for i=1:20
gtsam.plotPose3(initial.at(i).compose(camera_transform));
gtsam.plotPose3(initial.atPose3(i).compose(camera_transform));
end
xlabel('x (m)');
ylabel('y (m)');
disp('Transform before optimization');
initial.at(1000)
initial.atPose3(1000)
params = LevenbergMarquardtParams;
params.setAbsoluteErrorTol(1e-15);
@ -112,7 +112,7 @@ optimizer = LevenbergMarquardtOptimizer(fg, initial, params);
result = optimizer.optimizeSafely();
disp('Transform after optimization');
result.at(1000)
result.atPose3(1000) % results is empty here. optimizer doesn't generate result?
axis([0 25 0 25 0 10]);
axis equal

18
matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
View File

@ -86,17 +86,17 @@ for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
initial.insert(i,result.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
initial.insert(i,result.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -127,10 +127,10 @@ for i=1:20
hold on;
%% plot results
result_camera_transform = result.at(1000);
result_camera_transform = result.atPose3(1000);
for j=1:i
gtsam.plotPose3(result.at(j));
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
gtsam.plotPose3(result.atPose3(j));
gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
@ -143,10 +143,10 @@ for i=1:20
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
plotPoint3(result.atPoint3(l),'g');
end
ty = result.at(1000).translation().y();
ty = result.atPose3(1000).translation().y();
text(5,5,5,sprintf('Y-Transform: %0.2g',ty));
if(write_video)
@ -168,7 +168,7 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(1000)
result.atPose3(1000)

2
matlab/unstable_examples/plot_projected_landmarks.m
View File

@ -23,7 +23,7 @@ z = zeros(1,nrMeasurements);
for i = 0:measurement_keys.size-1
key = measurement_keys.at(i);
key_index = gtsam.symbolIndex(key);
p = landmarks.at(gtsam.symbol('l',key_index));
p = landmarks.atPoint3(gtsam.symbol('l',key_index));
x(i+1) = p.x;
y(i+1) = p.y;

2
matlab/unstable_examples/project_landmarks.m
View File

@ -8,7 +8,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
measurements = Values;
for i=1:size(landmarks)-1
z = camera.project(landmarks.at(symbol('l',i)));
z = camera.project(landmarks.atPoint3(symbol('l',i)));
% check bounding box
if z.x < 0 || z.x > 1280

12
matlab/unstable_examples/testTSAMFactors.m
View File

@ -48,12 +48,12 @@ optimizer = LevenbergMarquardtOptimizer(graph, initial, params);
result = optimizer.optimize();
% Check result
CHECK('error',result.at(100).equals(b1,1e-5))
CHECK('error',result.at(10).equals(origin,1e-5))
CHECK('error',result.at(1).equals(Point2(0,1),1e-5))
CHECK('error',result.at(2).equals(Point2(0,1),1e-5))
CHECK('error',result.at(20).equals(origin,1e-5))
CHECK('error',result.at(200).equals(b2,1e-5))
CHECK('error',result.atPose2(100).equals(b1,1e-5))
CHECK('error',result.atPose2(10).equals(origin,1e-5))
CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5))
CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5))
CHECK('error',result.atPose2(20).equals(origin,1e-5))
CHECK('error',result.atPose2(200).equals(b2,1e-5))
% Check error
CHECK('error',abs(graph.error(result))<1e-9)

154
tests/testPreconditioner.cpp
View File

@ -28,86 +28,98 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
static GaussianFactorGraph createSimpleGaussianFactorGraph() {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2);
return fg;
TEST( PCGsolver, verySimpleLinearSystem) {
// Ax = [4 1][u] = [1] x0 = [2]
// [1 3][v] [2] [1]
//
// exact solution x = [1/11, 7/11]';
//
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2));
// Exact solution already known
VectorValues exactSolution;
exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished());
//exactSolution.print("Exact");
// Solve the system using direct method
VectorValues deltaDirect = simpleGFG.optimize();
EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7));
//deltaDirect.print("Direct");
// Solve the system using Preconditioned Conjugate Gradient solver
// Common PCG parameters
gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared<gtsam::PCGSolverParameters>();
pcg->setMaxIterations(500);
pcg->setEpsilon_abs(0.0);
pcg->setEpsilon_rel(0.0);
//pcg->setVerbosity("ERROR");
// With Dummy preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7));
//deltaPCGDummy.print("PCG Dummy");
// With Block-Jacobi preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
// It takes more than 1000 iterations for this test
pcg->setMaxIterations(1500);
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5));
//deltaPCGJacobi.print("PCG Jacobi");
}
/* ************************************************************************* */
// Copy of BlockJacobiPreconditioner::build
std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
{
const size_t n = keyInfo.size();
std::vector<size_t> dims_ = keyInfo.colSpec();
TEST(PCGSolver, simpleLinearSystem) {
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
//SharedDiagonal unit2 = noiseModel::Unit::Create(2);
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
/* prepare the buffer of block diagonals */
std::vector<Matrix> blocks; blocks.reserve(n);
// Expected solution
VectorValues expectedSolution;
expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished());
expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished());
expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished());
//expectedSolution.print("Expected");
/* allocate memory for the factorization of block diagonals */
size_t nnz = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t dim = dims_[i];
blocks.push_back(Matrix::Zero(dim, dim));
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim;
}
// Solve the system using direct method
VectorValues deltaDirect = simpleGFG.optimize();
EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5));
//deltaDirect.print("Direct");
/* compute the block diagonal by scanning over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Ai = jf->getA(it);
blocks[entry.index()] += (Ai.transpose() * Ai);
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Hii = hf->info(it, it).selfadjointView();
blocks[entry.index()] += Hii;
}
}
else {
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
// Solve the system using Preconditioned Conjugate Gradient solver
// Common PCG parameters
gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared<gtsam::PCGSolverParameters>();
pcg->setMaxIterations(500);
pcg->setEpsilon_abs(0.0);
pcg->setEpsilon_rel(0.0);
//pcg->setVerbosity("ERROR");
return blocks;
}
// With Dummy preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
//deltaPCGDummy.print("PCG Dummy");
/* ************************************************************************* */
TEST( Preconditioner, buildBlocks ) {
// Create simple Gaussian factor graph and initial values
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
Values initial;
initial.insert(0,Point2(4, 5));
initial.insert(1,Point2(0, 1));
initial.insert(2,Point2(-5, 7));
// With Block-Jacobi preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
//deltaPCGJacobi.print("PCG Jacobi");
// Expected Hessian block diagonal matrices
std::map<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> actualHessian = buildBlocks(gfg, KeyInfo(gfg));
// Compare the number of block diagonal matrices
EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size());
// Compare the values of matrices
std::map<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::const_iterator it2 = actualHessian.begin();
for(; it1!=expectedHessian.end(); it1++, it2++)
EXPECT(assert_equal(it1->second, *it2));
}
/* ************************************************************************* */

6
wrap/Argument.cpp
View File

@ -78,11 +78,11 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
string cppType = type.qualifiedName("::");
string matlabUniqueType = type.qualifiedName();
if (is_ptr)
if (is_ptr && type.category != Qualified::EIGEN)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
file.oss << "boost::shared_ptr<" << cppType << "> " << name
<< " = unwrap_shared_ptr< ";
else if (is_ref)
else if (is_ref && type.category != Qualified::EIGEN)
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";
else
@ -94,7 +94,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << cppType << " " << name << " = unwrap< ";
file.oss << cppType << " >(" << matlabName;
if (is_ptr || is_ref)
if( (is_ptr || is_ref) && type.category != Qualified::EIGEN)
file.oss << ", \"ptr_" << matlabUniqueType << "\"";
file.oss << ");" << endl;
}

1
wrap/Module.cpp
View File

@ -156,6 +156,7 @@ void Module::parseMarkup(const std::string& data) {
>> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)]
>> ch_p(';')
[push_back_a(forward_declarations, fwDec)]
[assign_a(cls,cls0)] // also clear class to avoid partial parse
[assign_a(fwDec, fwDec0)];
Rule module_content_p = basic.comments_p | include_p | class_p

17
wrap/matlab.h
View File

@ -425,3 +425,20 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro
boost::shared_ptr<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
return *spp;
}
//// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
//template <>
//Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {
// bool unwrap_shared_ptr_Vector_attempted = false;
// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer");
// return Vector();
//}
//// throw an error if unwrap_shared_ptr is attempted for an Eigen Matrix
//template <>
//Matrix unwrap_shared_ptr<Matrix>(const mxArray* obj, const string& propertyName) {
// bool unwrap_shared_ptr_Matrix_attempted = false;
// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer");
// return Matrix();
//}

22
wrap/tests/expected/geometry_wrapper.cpp
View File

@ -363,7 +363,7 @@ void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mx
typedef boost::shared_ptr<Test> Shared;
checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
obj->arg_EigenConstRef(value);
}
@ -707,7 +707,7 @@ void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplatePoint2> Shared;
checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix t = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
}
@ -736,7 +736,7 @@ void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplatePoint2> Shared;
checkArguments("templatedMethodVector",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector");
Vector t = unwrap< Vector >(in[1]);
out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
}
@ -795,7 +795,7 @@ void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("accept_T",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
obj->accept_T(value);
}
@ -804,7 +804,7 @@ void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, co
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("accept_Tptr",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
obj->accept_Tptr(value);
}
@ -842,7 +842,7 @@ void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("return_T",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->return_T(value));
}
@ -851,7 +851,7 @@ void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, co
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("return_Tptr",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
{
SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value));
out[0] = wrap_shared_ptr(ret,"Matrix");
@ -863,8 +863,8 @@ void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, co
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("return_ptrs",nargout,nargin-1,2);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
boost::shared_ptr<Matrix> p2 = unwrap_shared_ptr< Matrix >(in[2], "ptr_Matrix");
Matrix p1 = unwrap< Matrix >(in[1]);
Matrix p2 = unwrap< Matrix >(in[2]);
pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2);
{
SharedMatrix* ret = new SharedMatrix(pairResult.first);
@ -881,7 +881,7 @@ void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix t = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
}
@ -910,7 +910,7 @@ void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("templatedMethodVector",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector");
Vector t = unwrap< Vector >(in[1]);
out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
}

22
wrap/tests/expected2/geometry_wrapper.cpp
View File

@ -335,7 +335,7 @@ void Test_arg_EigenConstRef_22(int nargout, mxArray *out[], int nargin, const mx
typedef boost::shared_ptr<Test> Shared;
checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
obj->arg_EigenConstRef(value);
}
@ -679,7 +679,7 @@ void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplatePoint2> Shared;
checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix t = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
}
@ -708,7 +708,7 @@ void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplatePoint2> Shared;
checkArguments("templatedMethodVector",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector");
Vector t = unwrap< Vector >(in[1]);
out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
}
@ -767,7 +767,7 @@ void MyTemplateMatrix_accept_T_63(int nargout, mxArray *out[], int nargin, const
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("accept_T",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
obj->accept_T(value);
}
@ -776,7 +776,7 @@ void MyTemplateMatrix_accept_Tptr_64(int nargout, mxArray *out[], int nargin, co
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("accept_Tptr",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
obj->accept_Tptr(value);
}
@ -814,7 +814,7 @@ void MyTemplateMatrix_return_T_67(int nargout, mxArray *out[], int nargin, const
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("return_T",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->return_T(value));
}
@ -823,7 +823,7 @@ void MyTemplateMatrix_return_Tptr_68(int nargout, mxArray *out[], int nargin, co
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("return_Tptr",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix value = unwrap< Matrix >(in[1]);
{
SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value));
out[0] = wrap_shared_ptr(ret,"Matrix");
@ -835,8 +835,8 @@ void MyTemplateMatrix_return_ptrs_69(int nargout, mxArray *out[], int nargin, co
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("return_ptrs",nargout,nargin-1,2);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
boost::shared_ptr<Matrix> p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
boost::shared_ptr<Matrix> p2 = unwrap_shared_ptr< Matrix >(in[2], "ptr_Matrix");
Matrix p1 = unwrap< Matrix >(in[1]);
Matrix p2 = unwrap< Matrix >(in[2]);
pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2);
{
SharedMatrix* ret = new SharedMatrix(pairResult.first);
@ -853,7 +853,7 @@ void MyTemplateMatrix_templatedMethod_70(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix t = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
}
@ -882,7 +882,7 @@ void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplateMatrix> Shared;
checkArguments("templatedMethodVector",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector");
Vector t = unwrap< Vector >(in[1]);
out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
}

2
wrap/tests/geometry.h
View File

@ -1,7 +1,7 @@
// comments!
class VectorNotEigen;
class ns::OtherClass;
virtual class ns::OtherClass;
namespace gtsam {

98
wrap/tests/testClass.cpp
View File

@ -160,9 +160,103 @@ TEST( Class, Grammar ) {
EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category);
}
/* ************************************************************************* */
//******************************************************************************
TEST( Class, TemplateSubstition ) {
using classic::space_p;
// Create type grammar that will place result in cls
Class cls;
Template t;
ClassGrammar g(cls, t);
string markup(string("template<T = {void, double, Matrix, Point3}>"
"class Point2 {"
" T myMethod(const T& t) const;"
"};"));
EXPECT(parse(markup.c_str(), g, space_p).full);
// Method 2
Method m2 = cls.method("myMethod");
EXPECT(assert_equal("myMethod", m2.name()));
EXPECT(m2.isConst());
LONGS_EQUAL(1, m2.nrOverloads());
ReturnValue rv2 = m2.returnValue(0);
EXPECT(!rv2.isPair);
EXPECT(!rv2.type1.isPtr);
EXPECT(assert_equal("T", rv2.type1.name()));
EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv2.type1.category);
// Generate some expected values for qualified types
Qualified q_void("void", Qualified::VOID);
Qualified q_double("double", Qualified::BASIS);
Qualified q_Matrix("Matrix", Qualified::EIGEN);
Qualified q_Point3("Point3", Qualified::CLASS);
EXPECT_LONGS_EQUAL(4, t.nrValues());
EXPECT(t.argName()=="T");
EXPECT(t[0]==q_void);
EXPECT(t[1]==q_double);
EXPECT(t[2]==q_Matrix);
EXPECT(t[3]==q_Point3);
vector<Class> classes = cls.expandTemplate(t.argName(), t.argValues());
// check the number of new classes is four
EXPECT_LONGS_EQUAL(4, classes.size());
// check return types
EXPECT( classes[0].method("myMethod").returnValue(0).type1 == q_void);
EXPECT( classes[1].method("myMethod").returnValue(0).type1 == q_double);
EXPECT( classes[2].method("myMethod").returnValue(0).type1 == q_Matrix);
EXPECT( classes[3].method("myMethod").returnValue(0).type1 == q_Point3);
// check the argument types
EXPECT( classes[0].method("myMethod").argumentList(0)[0].type == q_void);
EXPECT( classes[1].method("myMethod").argumentList(0)[0].type == q_double);
EXPECT( classes[2].method("myMethod").argumentList(0)[0].type == q_Matrix);
EXPECT( classes[3].method("myMethod").argumentList(0)[0].type == q_Point3);
}
//******************************************************************************
TEST(Class, Template) {
using classic::space_p;
// Create type grammar that will place result in cls
Class cls;
Template t;
ClassGrammar g(cls, t);
string markup(
string(
"template<T = {Vector, Matrix}>"
" virtual class PriorFactor : gtsam::NoiseModelFactor {"
" PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); "
" T prior() const; "
" void serialize() const; "
"};"));
EXPECT(parse(markup.c_str(), g, space_p).full);
}
//******************************************************************************
TEST( Class, Virtualness ) {
using classic::space_p;
Class cls;
Template t;
ClassGrammar g(cls, t);
string markup("virtual class Point3 {};");
EXPECT(parse(markup.c_str(), g, space_p).full);
EXPECT(cls.isVirtual);
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
//******************************************************************************