closed-form formula of Pose2's expmap/logmap derivatives
thduynguyen
parent
02e4f920d0
commit
30afbc5f1d
|
|
@ -1,5 +1,4 @@
|
|||
/build*
|
||||
/doc*
|
||||
*.pyc
|
||||
*.DS_Store
|
||||
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
|
||||
|
|
|
|||
|
|
@ -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 *)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<Vector, Vector3>(
|
||||
boost::function<Vector(const Vector&)>(
|
||||
boost::bind(testDexpL, _1)), Vector(zero(3)), 1e-2);
|
||||
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(testDexpL, w, _1), zero(3), 1e-2);
|
||||
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
|
||||
|
||||
Matrix actualDexpInvL = Pose2::dexpInvL(w);
|
||||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
|
||||
|
||||
// test case where alpha = 0
|
||||
Matrix actualDexpL0 = Pose2::dexpL(w0);
|
||||
Matrix expectedDexpL0 = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(testDexpL, w0, _1), zero(3), 1e-2);
|
||||
EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5));
|
||||
|
||||
Matrix actualDexpInvL0 = Pose2::dexpInvL(w0);
|
||||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue