diff --git a/.cproject b/.cproject index 1e86e3c10..c3285932f 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,7 +600,6 @@ make - testBinaryBayesNet.run true false @@ -647,7 +647,6 @@ make - testSymbolicBayesNet.run true false @@ -655,7 +654,6 @@ make - tests/testSymbolicFactor.run true false @@ -663,7 +661,6 @@ make - testSymbolicFactorGraph.run true false @@ -679,7 +676,6 @@ make - tests/testBayesTree true false @@ -1029,6 +1025,14 @@ true true + + make + -j4 + testMagFactor.run + true + true + true + make -j5 @@ -1135,7 +1139,6 @@ make - testErrors.run true false @@ -1365,46 +1368,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1487,6 +1450,7 @@ make + testSimulated2DOriented.run true false @@ -1526,6 +1490,7 @@ make + testSimulated2D.run true false @@ -1533,6 +1498,7 @@ make + testSimulated3D.run true false @@ -1546,6 +1512,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1803,7 +1809,6 @@ cpack - -G DEB true false @@ -1811,7 +1816,6 @@ cpack - -G RPM true false @@ -1819,7 +1823,6 @@ cpack - -G TGZ true false @@ -1827,7 +1830,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2706,7 +2708,6 @@ make - testGraph.run true false @@ -2714,7 +2715,6 @@ make - testJunctionTree.run true false @@ -2722,7 +2722,6 @@ make - testSymbolicBayesNetB.run true false @@ -3274,6 +3273,7 @@ make + tests/testGaussianISAM2 true false diff --git a/.gitignore b/.gitignore index f3f1efd5b..d46bddd10 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,4 @@ /build* -/doc* *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs new file mode 100644 index 000000000..b700d0a81 --- /dev/null +++ b/.settings/org.eclipse.cdt.core.prefs @@ -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 diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 3ea6a6318..ff1f1b692 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -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 diff --git a/doc/Mathematica/dexpInvL_SE2.nb b/doc/Mathematica/dexpInvL_SE2.nb new file mode 100644 index 000000000..d15932768 --- /dev/null +++ b/doc/Mathematica/dexpInvL_SE2.nb @@ -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 *) diff --git a/gtsam.h b/gtsam.h index 67b3f2888..5f5f4c5b1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,12 +156,6 @@ virtual class Value { size_t dim() const; }; -class Vector3 { - Vector3(Vector v); -}; -class Vector6 { - Vector6(Vector v); -}; #include class LieScalar { // Standard constructors @@ -1230,6 +1224,7 @@ class VectorValues { #include 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 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 @@ -2149,7 +2160,7 @@ class NonlinearISAM { #include #include -template +template 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 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 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); }; diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 5324ad410..ba665e6e5 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -19,7 +19,12 @@ #include +#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 #include #include diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 500f83ec3..5cd4cc8dd 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -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 #include #include diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 36e020bcb..ec63c733d 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -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 #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index b872aa08b..a9ef8fe10 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -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) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 1be8408d2..74cd248a1 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -34,6 +34,7 @@ namespace gtsam { typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors +typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix Vector4; @@ -42,6 +43,7 @@ typedef Eigen::Matrix Vector6; typedef Eigen::Matrix Vector7; typedef Eigen::Matrix Vector8; typedef Eigen::Matrix Vector9; +typedef Eigen::Matrix Vector10; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index e50e3af35..6abc98642 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,9 +17,32 @@ */ #include +#ifdef GTSAM_USE_TBB +#include +#endif namespace gtsam { - GTSAM_EXPORT FastMap > debugFlags; +GTSAM_EXPORT FastMap > 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; +} } diff --git a/gtsam/base/debug.h b/gtsam/base/debug.h index f416bd826..e21efcc83 100644 --- a/gtsam/base/debug.h +++ b/gtsam/base/debug.h @@ -43,6 +43,10 @@ namespace gtsam { GTSAM_EXTERN_EXPORT FastMap > 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 diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index ccd061d7c..368ae6c98 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -49,7 +49,7 @@ Vector4 Cal3Bundler::k() const { /* ************************************************************************* */ Vector3 Cal3Bundler::vector() const { - return (Vector(3) << f_, k1_, k2_).finished(); + return Vector3(f_, k1_, k2_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index cfbce2b28..12060c12d 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -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; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index f9292d4f6..37c156743 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -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 /// @{ diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 5bdf89856..8b7c1abf4 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -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(); } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 4513346d8..aea50208a 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -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) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index aacb5e84c..7684bb088 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -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(); } diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index b47153547..6ca778dcc 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -34,6 +34,7 @@ namespace gtsam { public: + enum { dimension = 6 }; typedef boost::shared_ptr 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 : public internal::Manifold { + }; + } // \ namespace gtsam diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index c2b690496..062178fea 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -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; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index c7cb0b958..375122191 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -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; /// @} diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index b276d9dce..8c60aa4d6 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -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 diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a32f84269..fa3606704 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -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(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0125e642a..a9b07faf1 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -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 /// @{ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2189f5030..80e1d2d2a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -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); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index c1dc93b22..80121bf9e 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -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); } /// @} diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 232661360..0b88a70c7 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -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); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 3167db0c3..6bd35f3cd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -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 diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index cd0c8ee9a..9170f8282 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -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; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 4d68f03e8..dd7adbb04 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -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_); } /// @} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 7471dca9d..5b4a8b33e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -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); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index f12d96899..da860b226 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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( + 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( + 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); diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index cfb103c5d..2b9e5c046 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -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( + boost::function( + 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; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index eebe2d60a..c77509b91 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -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(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(K)).project(point); } /* ************************************************************************* */ -TEST( StereoCamera, Dproject_stereo_point) +TEST( StereoCamera, Dproject) { - Matrix expected = numericalDerivative22(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)); } diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 0505f6c01..5e7e08f61 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -2,7 +2,7 @@ * ConjugateGradientSolver.cpp * * Created on: Jun 4, 2014 - * Author: ydjian + * Author: Yong-Dian Jian */ #include diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 6e8509309..91b58c2d4 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -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 @@ -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 V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { @@ -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 diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8c8e2cb2b..d7a793d50 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -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; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 54e721cd7..2879e0a4c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -254,6 +254,7 @@ namespace gtsam { map GaussianFactorGraph::hessianBlockDiagonal() const { map blocks; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!factor) continue; map BD = factor->hessianBlockDiagonal(); map::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) { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f282682b3..6d7f3c2e1 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -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 > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 62d84925c..8af5277e2 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -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 diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a63bbf473..566a98fc2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 92f43b6be..d33c5e07c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -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; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..ffb744239 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -2,20 +2,14 @@ * PCGSolver.cpp * * Created on: Feb 14, 2012 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include -//#include -//#include -//#include -//#include #include #include -//#include -//#include -//#include -//#include + #include #include #include @@ -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(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(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 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(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(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, diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..4c3006a3f 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -2,7 +2,8 @@ * Preconditioner.cpp * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include @@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { const Eigen::Map R(ptr, d, d); Eigen::Map b(dst, d, 1); - R.triangularView().solveInPlace(b); + R.triangularView().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 hessianMap = gf->hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); - } + std::map 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; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..623b29863 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -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; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index cba65580e..3a2cd0fd4 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -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(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) { diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 694163ede..950c6ce63 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -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 // + EXPECT( assert_equal((Matrix)numericalDerivative11 // (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 // + EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 88a92d181..2b014e0b8 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -516,8 +516,8 @@ struct GenerateFunctionalNode: Argument, Base { /// Given df/dT, multiply in dT/dA and continue reverse AD process // Cols is always known at compile time - template - void reverseAD4(const Eigen::Matrix & dFdT, + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD4(dFdT, jacobians); This::trace.reverseAD1(dFdT * This::dTdA, jacobians); diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index a755ab838..526ef1478 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -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 trace; T value(traceExecution(values, trace, traceStorage)); trace.startReverseAD1(jacobians); + +#ifdef _MSC_VER + delete[] traceStorage; +#endif + return value; } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b92e54143..372bced8e 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -112,6 +112,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + Vector Values::atFixed(Key j, size_t n) { + switch (n) { + case 1: return at(j); + case 2: return at(j); + case 3: return at(j); + case 4: return at(j); + case 5: return at(j); + case 6: return at(j); + case 7: return at(j); + case 8: return at(j); + case 9: return at(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(j,v); break; + case 2: insert(j,v); break; + case 3: insert(j,v); break; + case 4: insert(j,v); break; + case 5: insert(j,v); break; + case 6: insert(j,v); break; + case 7: insert(j,v); break; + case 8: insert(j,v); break; + case 9: insert(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) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d00baa0d9..dcbc2a433 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -180,6 +180,13 @@ namespace gtsam { template 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(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 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(j,c); } + /// overloaded insert version that also specifies a chart template void insert(Key j, const ValueType& val); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ceb2f70df..b798c6321 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -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(key1))); + CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index ecf8adfec..243331dc1 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -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 diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index cd42b8d4f..6abfe4336 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -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} diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt new file mode 100644 index 000000000..99a4b814e --- /dev/null +++ b/gtsam_unstable/linear/CMakeLists.txt @@ -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) diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h new file mode 100644 index 000000000..e6fec98b7 --- /dev/null +++ b/gtsam_unstable/linear/LinearEquality.h @@ -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 + +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 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, specifying the + * collection of keys and matrices making up the factor. */ + template + 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( + boost::make_shared(*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 + diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h new file mode 100644 index 000000000..fd9191250 --- /dev/null +++ b/gtsam_unstable/linear/LinearEqualityFactorGraph.h @@ -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 +#include + +namespace gtsam { + +class LinearEqualityFactorGraph : public FactorGraph { +public: + typedef boost::shared_ptr shared_ptr; +}; + +} // namespace gtsam + diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h new file mode 100644 index 000000000..24ad8545c --- /dev/null +++ b/gtsam_unstable/linear/LinearInequality.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequality.h + * @brief: LinearInequality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +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 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, specifying the + * collection of keys and matrices making up the factor. */ + template + 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( + boost::make_shared(*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 + diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h new file mode 100644 index 000000000..b72f2cc3c --- /dev/null +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -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 +#include + +namespace gtsam { + +class LinearInequalityFactorGraph: public FactorGraph { +private: + typedef FactorGraph Base; + +public: + typedef boost::shared_ptr 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 + diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h new file mode 100644 index 000000000..111ab506f --- /dev/null +++ b/gtsam_unstable/linear/QP.h @@ -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 +#include +#include + +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 + diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp new file mode 100644 index 000000000..f0eb7d7fb --- /dev/null +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -0,0 +1,252 @@ +/* + * QPSolver.cpp + * @brief: + * @date: Apr 15, 2014 + * @author: thduynguyen + */ + +#include +#include +#include + +#include + +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 > Aterms = collectDualJacobians + < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); + std::vector > 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(Aterms, b, noiseModel::Constrained::All(b.rows())); + } + else { + return boost::make_shared(); + } +} + +//****************************************************************************** +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 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; factorIxgetb()[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 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 */ diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h new file mode 100644 index 000000000..f7a575f8c --- /dev/null +++ b/gtsam_unstable/linear/QPSolver.h @@ -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 +#include + +#include +#include + +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 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 + std::vector > collectDualJacobians(Key key, + const FactorGraph& graph, + const VariableIndex& variableIndex) const { + std::vector > 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 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 solutions + */ + std::pair optimize( + const VectorValues& initialValues) const; + +}; + +} /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/CMakeLists.txt b/gtsam_unstable/linear/tests/CMakeLists.txt new file mode 100644 index 000000000..43df23daa --- /dev/null +++ b/gtsam_unstable/linear/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(linear_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp new file mode 100644 index 000000000..bf41743a2 --- /dev/null +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -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 +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) + +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); + + // 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);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp new file mode 100644 index 000000000..8fca61ca4 --- /dev/null +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -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 +#include +#include + +#include + +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 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); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index f1e94dcb4..a74a212c3 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -32,6 +32,8 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { +protected: + T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices diff --git a/gtsam_unstable/nonlinear/expressions.h b/gtsam_unstable/nonlinear/expressions.h index 2490100d6..18d0d5d8f 100644 --- a/gtsam_unstable/nonlinear/expressions.h +++ b/gtsam_unstable/nonlinear/expressions.h @@ -13,8 +13,7 @@ namespace gtsam { // Generics - -template +template Expression between(const Expression& t1, const Expression& t2) { return Expression(t1, &T::between, t2); } diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index 031baea3d..7badc9dd7 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -20,10 +20,7 @@ typedef Expression Rot2_; typedef Expression 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_; typedef Expression 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 diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index e205d918c..32f61e47f 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -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; diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m index 9fd2df463..11c4253de 100644 --- a/matlab/gtsam_examples/MonocularVOExample.m +++ b/matlab/gtsam_examples/MonocularVOExample.m @@ -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) diff --git a/matlab/gtsam_tests/testPriorFactor.m b/matlab/gtsam_tests/testPriorFactor.m new file mode 100644 index 000000000..4d567a6ce --- /dev/null +++ b/matlab/gtsam_tests/testPriorFactor.m @@ -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); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e08019610..1a6856a9a 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,5 +1,8 @@ % Test runner script - runs each test +% display 'Starting: testPriorFactor' +% testPriorFactor + display 'Starting: testValues' testValues diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index 1b05d2877..b0b37ee33 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -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 diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index 1db60a5ad..cb13eacee 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -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); diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m index 93e5dce0c..7535447df 100644 --- a/matlab/unstable_examples/SmartRangeFactorExample.m +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -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); diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index 236327b05..c9e912ea4 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -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) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index 834472a7d..fd4a17469 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -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; diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 410b7df0f..669073e56 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -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 diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 169736f4b..8edcfade7 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -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) diff --git a/matlab/unstable_examples/plot_projected_landmarks.m b/matlab/unstable_examples/plot_projected_landmarks.m index cfe08ec54..6b8101844 100644 --- a/matlab/unstable_examples/plot_projected_landmarks.m +++ b/matlab/unstable_examples/plot_projected_landmarks.m @@ -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; diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index b11c43bc5..629c6d994 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -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 diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index abdfc5922..341725723 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -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) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index ad2c0767a..43b4935fc 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -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(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + 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(); + // 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 buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) -{ - const size_t n = keyInfo.size(); - std::vector 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 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(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(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(); + 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(); + 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(); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); - // Expected Hessian block diagonal matrices - std::map expectedHessian =gfg.hessianBlockDiagonal(); - - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector 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::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - for(; it1!=expectedHessian.end(); it1++, it2++) - EXPECT(assert_equal(it1->second, *it2)); } /* ************************************************************************* */ diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 1f57917d8..848998eb0 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -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; } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 55fd13715..092c732f7 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -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 diff --git a/wrap/matlab.h b/wrap/matlab.h index 23f391903..1639876cc 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -425,3 +425,20 @@ boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& pro boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); return *spp; } + +//// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector +//template <> +//Vector unwrap_shared_ptr(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(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(); +//} + diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 527600b47..585918f20 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -363,7 +363,7 @@ void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mx typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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 Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } @@ -736,7 +736,7 @@ void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } @@ -795,7 +795,7 @@ void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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 Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr 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 Shared; checkArguments("return_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr 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 Shared; checkArguments("return_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr 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 Shared; checkArguments("return_ptrs",nargout,nargin-1,2); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); - boost::shared_ptr 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 Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } @@ -910,7 +910,7 @@ void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index 82926e2ce..4b41f3958 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -335,7 +335,7 @@ void Test_arg_EigenConstRef_22(int nargout, mxArray *out[], int nargin, const mx typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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 Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } @@ -708,7 +708,7 @@ void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } @@ -767,7 +767,7 @@ void MyTemplateMatrix_accept_T_63(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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 Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr 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 Shared; checkArguments("return_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr 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 Shared; checkArguments("return_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr 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 Shared; checkArguments("return_ptrs",nargout,nargin-1,2); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); - boost::shared_ptr 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 Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } @@ -882,7 +882,7 @@ void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(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(t)); } diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 78e2a1dff..69bc7e3be 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -1,7 +1,7 @@ // comments! class VectorNotEigen; -class ns::OtherClass; +virtual class ns::OtherClass; namespace gtsam { diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 756b6668d..a133e15ac 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -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" + "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 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" + " 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); } -/* ************************************************************************* */ +//******************************************************************************