gtsam/doc/Mathematica/SimiliarityGroup.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[     13070,        375]
NotebookOptionsPosition[     12606,        355]
NotebookOutlinePosition[     12963,        371]
CellTagsIndexPosition[     12920,        368]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell[TextData[{
 "Similarity Group\n",
 StyleBox["Representation", "Chapter"],
 "\n",
 StyleBox["Exponential and Logmap\nRetract and localCoordinate", "Chapter"]
}], "Title",
 CellChangeTimes->{{3.6442457705813923`*^9, 3.644245851964954*^9}, {
   3.644245883900199*^9, 3.644245897451631*^9}, {3.644246409411936*^9, 
   3.6442464411218433`*^9}, {3.644246632965823*^9, 3.644246721355283*^9}, 
   3.644339337635804*^9, {3.6443397539003696`*^9, 3.644339805690949*^9}, {
   3.6443398507824793`*^9, 3.644339940436355*^9}, {3.644339982058689*^9, 
   3.64434014492003*^9}, {3.644340183085711*^9, 3.64434030003795*^9}, {
   3.644340442268632*^9, 3.644340457502075*^9}, {3.644340502643257*^9, 
   3.644340517682786*^9}, {3.644340590324332*^9, 3.644340590748303*^9}, {
   3.6443655095926237`*^9, 3.644365512168627*^9}}],

Cell[BoxData[
 RowBox[{"\[IndentingNewLine]", 
  RowBox[{
  Cell["Lie group generators for similarity transform:"], 
   "\[IndentingNewLine]", 
   RowBox[{"G1", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"G2", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"G3", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"G4", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"-", "1"}], ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"G5", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"G6", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", 
        RowBox[{"-", "1"}], ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"G7", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", 
        RowBox[{"-", "1"}]}], "}"}]}], "}"}]}]}]}]], "Input",
 CellChangeTimes->{{3.644340621916498*^9, 3.644340623238144*^9}, {
  3.64434081043055*^9, 3.644340996367342*^9}, {3.6443410615657*^9, 
  3.644341237564106*^9}, {3.644341297291617*^9, 3.6443413676185513`*^9}, {
  3.644341671605256*^9, 3.64434167330084*^9}, {3.644345191055595*^9, 
  3.644345216636923*^9}}],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.644369249242889*^9, 3.644369249253479*^9}}],

Cell[BoxData[{Cell["Lie vectors similarity3 can be described as:"], "\
\[IndentingNewLine]", 
 RowBox[{"u", " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{"x", ",", "y", ",", "z"}], "}"}]}], "\[IndentingNewLine]", 
 RowBox[{"v", " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{"w1", ",", "w2", ",", "w3"}], "}"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simLieVector", " ", "=", " ", 
   RowBox[{"{", 
    RowBox[{"u", ",", "v", ",", "lambda"}], "}"}]}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{"simMatrix", " ", "=", " ", 
  RowBox[{"Table", "[", 
   RowBox[{"0", ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", "4"}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simMatrix", "[", 
   RowBox[{"[", 
    RowBox[{
     RowBox[{"1", ";;", "3"}], ",", 
     RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", 
  RowBox[{"SkewSym", "[", "v", "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simMatrix", "[", 
   RowBox[{"[", 
    RowBox[{
     RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], " ", "=", " ", 
  "u"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simMatrix", "[", 
   RowBox[{"[", 
    RowBox[{"4", ",", "4"}], "]"}], "]"}], " ", "=", " ", 
  "lambda"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"MatrixForm", "[", "simMatrix", "]"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"simExponetialMap", " ", "=", " ", 
    RowBox[{"Series", "[", 
     RowBox[{"simMatrix", ",", 
      RowBox[{"{", 
       RowBox[{"u", ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", "2"}], "}"}]}], 
     "]"}]}], "*)"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", Cell["Exponential map, \
refer to Ethan Eade"], "\[IndentingNewLine]", 
 RowBox[{"v_skew", " ", "=", " ", 
  RowBox[{"SkewSym", 
   RowBox[{"(", "v", ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"theta", " ", "=", 
  RowBox[{"sqrt", 
   RowBox[{"(", 
    RowBox[{"Transpose", 
     RowBox[{
      RowBox[{"(", "v", ")"}], ".", "v"}]}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"X", " ", "=", " ", 
  RowBox[{"sin", 
   RowBox[{
    RowBox[{"(", "theta", ")"}], "/", "theta"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"Y", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{"1", "-", 
     RowBox[{"cos", 
      RowBox[{"(", "theta", ")"}]}]}], ")"}], "/", 
   RowBox[{"(", 
    RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"Z", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{"1", "-", "X"}], ")"}], "/", 
   RowBox[{"(", 
    RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"W", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{"0.5", " ", "-", " ", "Y"}], ")"}], "/", 
   RowBox[{"(", 
    RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"alpha", " ", "=", " ", 
  RowBox[{
   RowBox[{"lambda", "^", "2"}], " ", "/", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"lambda", "^", "2"}], " ", "+", " ", 
     RowBox[{"theta", "^", "2"}]}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"beta", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{
     RowBox[{"exp", 
      RowBox[{"(", 
       RowBox[{"-", "lambda"}], ")"}]}], "-", "1", "+", "lambda"}], ")"}], 
   "/", 
   RowBox[{"(", 
    RowBox[{"lambda", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"gamma", " ", "=", " ", 
  RowBox[{"Y", " ", "-", " ", 
   RowBox[{"lambda", ".", "Z"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"mu", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{"1", "-", "lambda", " ", "+", " ", 
     RowBox[{"0.5", "*", 
      RowBox[{"lambda", "^", "2"}]}], " ", "-", " ", 
     RowBox[{"exp", 
      RowBox[{"(", 
       RowBox[{"-", "lambda"}], ")"}]}]}], ")"}], "/", 
   RowBox[{"(", 
    RowBox[{"lambda", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"nu", " ", "=", " ", 
  RowBox[{"Z", "-", 
   RowBox[{"lambda", " ", "W"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"Av", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{"1", "-", 
     RowBox[{"exp", 
      RowBox[{"(", 
       RowBox[{"-", "lambda"}], ")"}]}]}], ")"}], "/", 
   "lambda"}]}], "\[IndentingNewLine]", 
 RowBox[{"Bv", " ", "=", " ", 
  RowBox[{
   RowBox[{"alpha", " ", ".", 
    RowBox[{"(", 
     RowBox[{"beta", " ", "-", " ", "lambda"}], ")"}]}], " ", "+", " ", 
   "lambda"}]}], "\[IndentingNewLine]", 
 RowBox[{"Cv", " ", "=", " ", 
  RowBox[{
   RowBox[{"alpha", " ", ".", 
    RowBox[{"(", 
     RowBox[{"mu", " ", "-", " ", "nv"}], ")"}]}], " ", "+", " ", 
   "nv"}]}], "\[IndentingNewLine]", 
 RowBox[{"a", " ", "=", " ", 
  RowBox[{"sin", 
   RowBox[{
    RowBox[{"(", "theta", ")"}], "/", "theta"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"b", " ", "=", " ", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{"1", "-", 
     RowBox[{"cos", 
      RowBox[{"(", "theta", ")"}]}]}], ")"}], "/", 
   RowBox[{"(", 
    RowBox[{"theta", "^", "2"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"R", " ", "=", " ", 
  RowBox[{
   RowBox[{"IdentityMatrix", "[", "3", "]"}], "+", " ", 
   RowBox[{"a", ".", "v_skew"}], " ", "+", " ", 
   RowBox[{"b", ".", "v_skew", ".", "v_skew"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"V", " ", "=", " ", 
  RowBox[{
   RowBox[{"Av", ".", 
    RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "+", 
   RowBox[{"Bv", ".", "v_skew"}], " ", "+", " ", 
   RowBox[{
   "Cv", ".", " ", "v_skew", ".", "v_skew"}]}]}], "\[IndentingNewLine]", 
 RowBox[{"simExponetialMap", " ", "=", " ", 
  RowBox[{"Table", "[", 
   RowBox[{"0", ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", "4"}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", "4"}], "}"}]}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simExponetialMap", "[", 
   RowBox[{"[", 
    RowBox[{
     RowBox[{"1", ";;", "3"}], ",", 
     RowBox[{"1", ";;", "3"}]}], "]"}], "]"}], " ", "=", " ", 
  "R"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simExponetialMap", "[", 
   RowBox[{"[", 
    RowBox[{
     RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], " ", "=", " ", 
  RowBox[{"V", ".", "u"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"simExponetialMap", "[", 
   RowBox[{"[", 
    RowBox[{"4", ",", "4"}], "]"}], "]"}], " ", "=", " ", 
  RowBox[{"exp", 
   RowBox[{"(", 
    RowBox[{"-", "lambda"}], ")"}]}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"MatrixForm", "[", "simExponetialMap", "]"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"f", "[", "lambda_", "]"}], " ", "=", " ", 
  "simExponetialMap"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"f", "'"}], "[", "lambda", "]"}], "\[IndentingNewLine]"}], "Input",
 CellChangeTimes->CompressedData["
1:eJwl0l1Ik3EUBvCZsrU2x7CtlFpoGaN3iiauLlpgBGJtqFntInGshlqBTX2x
bOUQsUnsQiizBB312sfLZvkxRSM0yYkxyJwSFtXaxvADIW3pkqHYnr8Xhx8H
DufhwEm5Yiwq3cHhcNKjBeN9Oc08+dIpk970HO4fNzggK54agbreFxNwReL/
BGc7XR44J3r4ZdurXpjPyhZg753quZ1R7eaSRWjlr/2GudzqMHzrexeBef4n
G3DoYh2Xj5zJzL0wKGvaB9lK5jBsLNVSUJV1IAP+Cf1SQrfQeAy2D9IG2M1z
lsNLKnkVfGTOJPadCdfArXvBZjhEfW+BTk9qG7xJF9qguFzzFE7ffvyK5Nvd
DpJrOzIAhauGcZi2RgehyGQhMoHLgl3Iq1cIodj2/nVC1NEPxYMwQJ2cgKr0
eR8sKzs9D9mBkiXo3sxbgXYlG4Ipz+6GYfxGfszuqJPtXi482+Hgk/5lbCLU
NYaIe9alMvhgrDAZ9l04r9ie8+RCTUu2Bcp5gSaYulVvg5ITGgYyumsOskff
Skyixf3Qb+2ZgXoL/RU6zwl+wOSu439hYqyZyPZXrMPKVm+cBPd1WAXw4Ge1
CB5ixhLgt9phBRQVZx+FacY6JWxQF6kgdf9fDpy6IVVDifN6AXzzcWYZal3U
KhyNKCOwVttAdP2c5kjxBxW34uB/2UIzug==
  "]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.644369188660811*^9, 3.644369188678254*^9}}]
}, Open  ]]
},
WindowSize->{812, 579},
WindowMargins->{{Automatic, 17}, {Automatic, 31}},
FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (December 4, \
2014)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[580, 22, 808, 14, 224, "Title"],
Cell[1391, 38, 3597, 96, 166, "Input"],
Cell[4991, 136, 92, 1, 28, InheritFromParent],
Cell[5086, 139, 7409, 210, 696, "Input"],
Cell[12498, 351, 92, 1, 28, InheritFromParent]
}, Open  ]]
}
]
*)

(* End of internal cache information *)