gtsam/doc/Mathematica/Quaternion-Logmap.nb

(* Content-type: application/vnd.wolfram.mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 10.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       158,          7]
NotebookDataLength[      6004,        217]
NotebookOptionsPosition[      5104,        179]
NotebookOutlinePosition[      5456,        195]
CellTagsIndexPosition[      5413,        192]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell["\<\
In Quaternion.h we have Logmap, but we have to be careful when qw approaches \
-1 (from above) or 1 (from below). The Taylor expansions below are the basis \
for the code.\
\>", "Text",
 CellChangeTimes->{{3.632651837171029*^9, 3.6326518973274307`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"angle", "=", 
  RowBox[{"2", 
   RowBox[{"ArcCos", "[", "qw", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.6326509558588057`*^9, 3.632650976842943*^9}}],

Cell[BoxData[
 RowBox[{"2", " ", 
  RowBox[{"ArcCos", "[", "qw", "]"}]}]], "Output",
 CellChangeTimes->{{3.6326509669341784`*^9, 3.6326509795921097`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"s", "=", 
  RowBox[{"Sqrt", "[", 
   RowBox[{"1", "-", 
    RowBox[{"qw", "*", "qw"}]}], "]"}]}]], "Input",
 CellChangeTimes->{{3.632650983796185*^9, 3.632650994132272*^9}}],

Cell[BoxData[
 SqrtBox[
  RowBox[{"1", "-", 
   SuperscriptBox["qw", "2"]}]]], "Output",
 CellChangeTimes->{3.63265099440246*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"factor", " ", "=", " ", 
  RowBox[{"angle", "/", "s"}]}]], "Input",
 CellChangeTimes->{{3.632650999925654*^9, 3.632651001339293*^9}, {
  3.632651070297429*^9, 3.632651071527272*^9}}],

Cell[BoxData[
 FractionBox[
  RowBox[{"2", " ", 
   RowBox[{"ArcCos", "[", "qw", "]"}]}], 
  SqrtBox[
   RowBox[{"1", "-", 
    SuperscriptBox["qw", "2"]}]]]], "Output",
 CellChangeTimes->{3.632651001671771*^9, 3.632651072007021*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Expand", "[", 
  RowBox[{"Series", "[", 
   RowBox[{
    FractionBox[
     RowBox[{"2", " ", 
      RowBox[{"ArcCos", "[", "qw", "]"}]}], 
     SqrtBox[
      RowBox[{"1", "-", 
       SuperscriptBox["qw", "2"]}]]], ",", 
    RowBox[{"{", 
     RowBox[{"qw", ",", "1", ",", "1"}], "}"}], ",", 
    RowBox[{"Assumptions", "->", 
     RowBox[{"(", 
      RowBox[{"qw", "<", "1"}], ")"}]}]}], "]"}], "]"}]], "Input",
 CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
  3.6326511716876993`*^9, 3.632651189491748*^9}, {3.632651248821335*^9, 
  3.632651267905816*^9}}],

Cell[BoxData[
 InterpretationBox[
  RowBox[{"2", "-", 
   FractionBox[
    RowBox[{"2", " ", 
     RowBox[{"(", 
      RowBox[{"qw", "-", "1"}], ")"}]}], "3"], "+", 
   InterpretationBox[
    SuperscriptBox[
     RowBox[{"O", "[", 
      RowBox[{"qw", "-", "1"}], "]"}], 
     RowBox[{"3", "/", "2"}]],
    SeriesData[$CellContext`qw, 1, {}, 0, 3, 2],
    Editable->False]}],
  SeriesData[$CellContext`qw, 1, {2, 0, 
    Rational[-2, 3]}, 0, 3, 2],
  Editable->False]], "Output",
 CellChangeTimes->{{3.632651102947558*^9, 3.632651118218814*^9}, {
  3.632651179610784*^9, 3.6326511898522263`*^9}, {3.632651249719887*^9, 
  3.632651268312502*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"ArcCos", "[", 
  RowBox[{"-", "1"}], "]"}]], "Input",
 CellChangeTimes->{{3.632651352754121*^9, 3.63265135286866*^9}}],

Cell[BoxData["\[Pi]"], "Output",
 CellChangeTimes->{3.632651353300222*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Expand", "[", 
  RowBox[{"Series", "[", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{"-", "2"}], 
      RowBox[{"ArcCos", "[", 
       RowBox[{"-", "qw"}], "]"}]}], 
     SqrtBox[
      RowBox[{"1", "-", 
       SuperscriptBox["qw", "2"]}]]], ",", 
    RowBox[{"{", 
     RowBox[{"qw", ",", 
      RowBox[{"-", "1"}], ",", "1"}], "}"}], ",", 
    RowBox[{"Assumptions", "->", 
     RowBox[{"(", 
      RowBox[{"qw", ">", 
       RowBox[{"-", "1"}]}], ")"}]}]}], "]"}], "]"}]], "Input",
 CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
  3.6326511716876993`*^9, 3.6326512088422937`*^9}, {3.632651301817163*^9, 
  3.6326513406015453`*^9}, {3.63265150259446*^9, 3.632651505055284*^9}, {
  3.632651744223112*^9, 3.632651772717318*^9}}],

Cell[BoxData[
 InterpretationBox[
  RowBox[{
   RowBox[{"-", "2"}], "-", 
   FractionBox[
    RowBox[{"2", " ", 
     RowBox[{"(", 
      RowBox[{"qw", "+", "1"}], ")"}]}], "3"], "+", 
   InterpretationBox[
    SuperscriptBox[
     RowBox[{"O", "[", 
      RowBox[{"qw", "+", "1"}], "]"}], 
     RowBox[{"3", "/", "2"}]],
    SeriesData[$CellContext`qw, -1, {}, 0, 3, 2],
    Editable->False]}],
  SeriesData[$CellContext`qw, -1, {-2, 0, 
    Rational[-2, 3]}, 0, 3, 2],
  Editable->False]], "Output",
 CellChangeTimes->{
  3.632651209181905*^9, 3.6326513025091133`*^9, {3.632651332608609*^9, 
   3.632651341031022*^9}, 3.632651506516138*^9, {3.632651746679185*^9, 
   3.632651773032124*^9}}]
}, Open  ]]
},
WindowSize->{808, 751},
WindowMargins->{{4, Automatic}, {Automatic, 4}},
FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (June 27, \
2014)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[558, 20, 263, 5, 49, "Text"],
Cell[CellGroupData[{
Cell[846, 29, 174, 4, 28, "Input"],
Cell[1023, 35, 154, 3, 28, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[1214, 43, 197, 5, 28, "Input"],
Cell[1414, 50, 129, 4, 40, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[1580, 59, 206, 4, 28, "Input"],
Cell[1789, 65, 233, 7, 59, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[2059, 77, 605, 17, 61, "Input"],
Cell[2667, 96, 645, 19, 48, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[3349, 120, 142, 3, 28, "Input"],
Cell[3494, 125, 74, 1, 28, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[3605, 131, 788, 22, 61, "Input"],
Cell[4396, 155, 692, 21, 48, "Output"]
}, Open  ]]
}
]
*)

(* End of internal cache information *)