gtsam/doc/Mathematica/dexpInvL_SE2.nb

608 lines
19 KiB
Mathematica

(* 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 *)