From 30afbc5f1d17e419ca7c9785391b5b8e0d16df95 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Fri, 19 Dec 2014 13:53:08 -0500 Subject: [PATCH] closed-form formula of Pose2's expmap/logmap derivatives --- .gitignore | 1 - doc/Mathematica/dexpInvL_SE2.nb | 607 +++++++++++++++++++++++++++++ gtsam/geometry/Pose2.cpp | 74 ++-- gtsam/geometry/tests/testPose2.cpp | 19 +- 4 files changed, 660 insertions(+), 41 deletions(-) create mode 100644 doc/Mathematica/dexpInvL_SE2.nb 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/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/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index bea2c54c2..719856f78 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -136,47 +136,51 @@ Matrix3 Pose2::adjointMap(const Vector& v) { /* ************************************************************************* */ Matrix3 Pose2::dexpL(const Vector3& v) { - // See Iserles05an, pg. 33. - // TODO: Duplicated code. Maybe unify them at higher Lie level? - static const int N = 10; // order of approximation - Matrix3 res = I_3x3; - Matrix3 ad_i = I_3x3; - Matrix3 ad = adjointMap(v); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad * ad_i; - fac = fac * (i+1); - // Since this is the left-trivialized version, we flip the signs of the odd terms - if (i%2 != 0) - res = res - 1.0 / fac * ad_i; - else - res = res + 1.0 / fac * ad_i; + 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(); } - return res; } /* ************************************************************************* */ Matrix3 Pose2::dexpInvL(const Vector3& v) { - // TODO: Duplicated code with Pose3. Maybe unify them at higher Lie level? - // Bernoulli numbers, from Wikipedia - static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); - static const int N = 5; // order of approximation - Matrix3 res = I_3x3; - Matrix3 ad_i = I_3x3; - Matrix3 ad = adjointMap(v); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad * ad_i; - fac = fac * i; - // Since this is the left-trivialized version, we flip the signs of the odd terms - // Note that all Bernoulli odd numbers are 0, except 1. - if (i==1) - res = res - B(i) / fac * ad_i; - else - res = res + B(i) / fac * ad_i; + 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(); } - return res; } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 5497f6e28..da860b226 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -193,25 +193,34 @@ TEST(Pose2, logmap_full) { } /* ************************************************************************* */ -Vector w = (Vector(3) << 0.1, 0.27, -0.2).finished(); +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 Vector& 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::function( - boost::bind(testDexpL, _1)), Vector(zero(3)), 1e-2); + 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)); } /* ************************************************************************* */