gtsam/doc/Mathematica/CalibratedStereoCamera.nb

(* 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[     16383,        482]
NotebookOptionsPosition[     15716,        454]
NotebookOutlinePosition[     16071,        470]
CellTagsIndexPosition[     16028,        467]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell["\<\
The derivative of CalibratedStereoCamera.project:\
\>", "Text",
 CellChangeTimes->{{3.5349648356912127`*^9, 3.534964846480523*^9}, {
  3.534983100980744*^9, 3.534983101484686*^9}, {3.535027340892766*^9, 
  3.535027363679287*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"H1", "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     FractionBox["1", "z"], 
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1", "0", 
          RowBox[{
           RowBox[{"-", " ", "xL"}], "/", "z"}]},
         {"1", "0", 
          RowBox[{
           RowBox[{"-", " ", "xR"}], "/", "z"}]},
         {"0", "1", 
          RowBox[{
           RowBox[{"-", " ", "y"}], "/", "z"}]}
        }], ")"}], ".", 
      RowBox[{"(", GridBox[{
         {"0", 
          RowBox[{"-", "z"}], "y", 
          RowBox[{"-", "1"}], "0", "0", "R00", "R10", "R20"},
         {"z", "0", 
          RowBox[{"-", "x"}], "0", 
          RowBox[{"-", "1"}], "0", "R01", "R11", "R21"},
         {
          RowBox[{"-", "y"}], "x", "0", "0", "0", 
          RowBox[{"-", "1"}], "R02", "R12", "R22"}
        }], ")"}]}]}], "//", "FullSimplify"}], "//", 
   "MatrixForm"}]}]], "Input",
 CellChangeTimes->{{3.5349631885090446`*^9, 3.5349632390951033`*^9}, {
   3.53496344228561*^9, 3.534963552777855*^9}, {3.53496365487344*^9, 
   3.53496369917972*^9}, 3.534963801932398*^9, {3.5349638327702837`*^9, 
   3.534963833842349*^9}, {3.5349638684957743`*^9, 3.534963869306225*^9}, {
   3.5349640018609343`*^9, 3.534964073635954*^9}, {3.534964139661661*^9, 
   3.534964141597094*^9}, {3.534964228457883*^9, 3.534964230497267*^9}, {
   3.534964522693956*^9, 3.534964554934105*^9}, 3.534979649216166*^9, {
   3.534979703414953*^9, 3.5349797245266523`*^9}, 3.534979754875834*^9, {
   3.5349801268943996`*^9, 3.534980128188809*^9}, {3.534981330976294*^9, 
   3.5349813327600317`*^9}, 3.534981567474765*^9, {3.534982081101321*^9, 
   3.534982107106247*^9}, {3.534982164393888*^9, 3.534982164510919*^9}, {
   3.534982374683118*^9, 3.534982376464725*^9}, {3.534985216789068*^9, 
   3.5349853102222767`*^9}, {3.535027369587408*^9, 3.535027381384465*^9}, {
   3.535027443509613*^9, 3.535027464114572*^9}, {3.535027568452767*^9, 
   3.5350275759632196`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      FractionBox[
       RowBox[{"xL", " ", "y"}], 
       SuperscriptBox["z", "2"]], 
      RowBox[{
       RowBox[{"-", "1"}], "-", 
       FractionBox[
        RowBox[{"x", " ", "xL"}], 
        SuperscriptBox["z", "2"]]}], 
      FractionBox["y", "z"], 
      RowBox[{"-", 
       FractionBox["1", "z"]}], "0", 
      FractionBox["xL", 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R02"}], " ", "xL"}], "+", 
        RowBox[{"R00", " ", "z"}]}], 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R12"}], " ", "xL"}], "+", 
        RowBox[{"R10", " ", "z"}]}], 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R22"}], " ", "xL"}], "+", 
        RowBox[{"R20", " ", "z"}]}], 
       SuperscriptBox["z", "2"]]},
     {
      FractionBox[
       RowBox[{"xR", " ", "y"}], 
       SuperscriptBox["z", "2"]], 
      RowBox[{
       RowBox[{"-", "1"}], "-", 
       FractionBox[
        RowBox[{"x", " ", "xR"}], 
        SuperscriptBox["z", "2"]]}], 
      FractionBox["y", "z"], 
      RowBox[{"-", 
       FractionBox["1", "z"]}], "0", 
      FractionBox["xR", 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R02"}], " ", "xR"}], "+", 
        RowBox[{"R00", " ", "z"}]}], 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R12"}], " ", "xR"}], "+", 
        RowBox[{"R10", " ", "z"}]}], 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R22"}], " ", "xR"}], "+", 
        RowBox[{"R20", " ", "z"}]}], 
       SuperscriptBox["z", "2"]]},
     {
      RowBox[{"1", "+", 
       FractionBox[
        SuperscriptBox["y", "2"], 
        SuperscriptBox["z", "2"]]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"x", " ", "y"}], 
        SuperscriptBox["z", "2"]]}], 
      RowBox[{"-", 
       FractionBox["x", "z"]}], "0", 
      RowBox[{"-", 
       FractionBox["1", "z"]}], 
      FractionBox["y", 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R02"}], " ", "y"}], "+", 
        RowBox[{"R01", " ", "z"}]}], 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R12"}], " ", "y"}], "+", 
        RowBox[{"R11", " ", "z"}]}], 
       SuperscriptBox["z", "2"]], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "R22"}], " ", "y"}], "+", 
        RowBox[{"R21", " ", "z"}]}], 
       SuperscriptBox["z", "2"]]}
    },
    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.534979650069955*^9, {3.534979704654621*^9, 3.5349797249719963`*^9}, 
   3.534979755407372*^9, 3.534980128803479*^9, 3.534981333789564*^9, 
   3.5349815682066193`*^9, 3.534982108180704*^9, 3.534982166031692*^9, 
   3.534982377250555*^9, {3.534985297869969*^9, 3.534985310666787*^9}, 
   3.5350273834131517`*^9, {3.535027445678911*^9, 3.535027465353374*^9}, {
   3.535027561035596*^9, 3.535027578459703*^9}, 3.535031349635189*^9}]
}, Open  ]],

Cell["\<\
To get rid of (1/z) factors, work in terms of u=x/z and v=x/z.\
\>", "Text",
 CellChangeTimes->{{3.534981922219325*^9, 3.534981963212936*^9}, {
  3.534983115196439*^9, 3.534983121940318*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"H2", "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      FractionBox["1", "z"], 
      RowBox[{
       RowBox[{"(", GridBox[{
          {"1", "0", 
           RowBox[{
            RowBox[{"-", " ", "xL"}], "/", "z"}]},
          {"1", "0", 
           RowBox[{
            RowBox[{"-", " ", "xR"}], "/", "z"}]},
          {"0", "1", 
           RowBox[{
            RowBox[{"-", " ", "y"}], "/", "z"}]}
         }], ")"}], ".", 
       RowBox[{"(", GridBox[{
          {"0", 
           RowBox[{"-", "z"}], "y", 
           RowBox[{"-", "1"}], "0", "0", "R00", "R10", "R20"},
          {"z", "0", 
           RowBox[{"-", "x"}], "0", 
           RowBox[{"-", "1"}], "0", "R01", "R11", "R21"},
          {
           RowBox[{"-", "y"}], "x", "0", "0", "0", 
           RowBox[{"-", "1"}], "R02", "R12", "R22"}
         }], ")"}]}]}], "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"xL", "\[Rule]", 
        RowBox[{"z", " ", "uL"}]}], ",", 
       RowBox[{"xR", "\[Rule]", 
        RowBox[{"z", " ", "uR"}]}], ",", 
       RowBox[{"y", "\[Rule]", 
        RowBox[{"z", " ", "v"}]}]}], "}"}]}], "//", "FullSimplify"}], " ", "//",
    "MatrixForm", " "}]}]], "Input",
 CellChangeTimes->{{3.534981575562448*^9, 3.534981660163035*^9}, {
   3.534981699433219*^9, 3.534981715884362*^9}, {3.534981811953741*^9, 
   3.534981812758938*^9}, {3.534981967549185*^9, 3.534981970084591*^9}, {
   3.5349821168936243`*^9, 3.5349821229726763`*^9}, {3.5349821691614027`*^9, 
   3.534982169838853*^9}, {3.5349823818506517`*^9, 3.5349823882411737`*^9}, {
   3.534982418562274*^9, 3.534982445450313*^9}, {3.5349824987688103`*^9, 
   3.5349825453911533`*^9}, 3.5349853401371813`*^9, {3.5349854168944473`*^9, 
   3.534985505039978*^9}, {3.5349855515891447`*^9, 3.5349855522595778`*^9}, {
   3.534985594640473*^9, 3.534985605102441*^9}, {3.534985635544286*^9, 
   3.5349856556893806`*^9}, {3.534985722650296*^9, 3.534985735961334*^9}, {
   3.5349862396524563`*^9, 3.534986241578967*^9}, {3.534986277578917*^9, 
   3.5349863148397903`*^9}, {3.534986786489544*^9, 3.534986789124526*^9}, {
   3.5349868534541187`*^9, 3.534986854897138*^9}, {3.53502738808462*^9, 
   3.5350274696107397`*^9}, {3.535027581254743*^9, 3.535027584027256*^9}, {
   3.5350276166332808`*^9, 3.535027627938361*^9}, {3.535027699887917*^9, 
   3.535027701552945*^9}, {3.535027782110402*^9, 3.5350277854136066`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"uL", " ", "v"}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{
         RowBox[{"uL", " ", "x"}], "+", "z"}], "z"]}], "v", 
      RowBox[{"-", 
       FractionBox["1", "z"]}], "0", 
      FractionBox["uL", "z"], 
      FractionBox[
       RowBox[{"R00", "-", 
        RowBox[{"R02", " ", "uL"}]}], "z"], 
      FractionBox[
       RowBox[{"R10", "-", 
        RowBox[{"R12", " ", "uL"}]}], "z"], 
      FractionBox[
       RowBox[{"R20", "-", 
        RowBox[{"R22", " ", "uL"}]}], "z"]},
     {
      RowBox[{"uR", " ", "v"}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{
         RowBox[{"uR", " ", "x"}], "+", "z"}], "z"]}], "v", 
      RowBox[{"-", 
       FractionBox["1", "z"]}], "0", 
      FractionBox["uR", "z"], 
      FractionBox[
       RowBox[{"R00", "-", 
        RowBox[{"R02", " ", "uR"}]}], "z"], 
      FractionBox[
       RowBox[{"R10", "-", 
        RowBox[{"R12", " ", "uR"}]}], "z"], 
      FractionBox[
       RowBox[{"R20", "-", 
        RowBox[{"R22", " ", "uR"}]}], "z"]},
     {
      RowBox[{"1", "+", 
       SuperscriptBox["v", "2"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"v", " ", "x"}], "z"]}], 
      RowBox[{"-", 
       FractionBox["x", "z"]}], "0", 
      RowBox[{"-", 
       FractionBox["1", "z"]}], 
      FractionBox["v", "z"], 
      FractionBox[
       RowBox[{"R01", "-", 
        RowBox[{"R02", " ", "v"}]}], "z"], 
      FractionBox[
       RowBox[{"R11", "-", 
        RowBox[{"R12", " ", "v"}]}], "z"], 
      FractionBox[
       RowBox[{"R21", "-", 
        RowBox[{"R22", " ", "v"}]}], "z"]}
    },
    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.534981661750978*^9, {3.534981700471121*^9, 3.5349817167574883`*^9}, 
   3.534981813153152*^9, 3.5349819705942793`*^9, {3.534982117522484*^9, 
   3.534982124251252*^9}, 3.534982170293386*^9, 3.534982388956635*^9, 
   3.534982446240321*^9, {3.534982502305121*^9, 3.534982546180998*^9}, 
   3.534985341687833*^9, {3.534985429323271*^9, 3.534985505574792*^9}, 
   3.534985553224812*^9, {3.534985595114938*^9, 3.534985605471074*^9}, {
   3.534985637118844*^9, 3.534985656675743*^9}, {3.534985725554123*^9, 
   3.534985736368793*^9}, 3.5349862441909113`*^9, 3.534986284242839*^9, 
   3.534986315973349*^9, 3.5349868083087063`*^9, 3.5349868556678743`*^9, {
   3.535027409664824*^9, 3.535027470084853*^9}, {3.535027561117285*^9, 
   3.535027585819845*^9}, {3.535027620794636*^9, 3.535027628250724*^9}, 
   3.535027702304246*^9, 3.535027785836504*^9, 3.535031349716421*^9}]
}, Open  ]],

Cell["Finally, avoid divides.", "Text",
 CellChangeTimes->{{3.534983093980698*^9, 3.5349831123126383`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"FullSimplify", "[", 
  RowBox[{
   RowBox[{"H2", "/.", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"1", "/", "z"}], "\[Rule]", "d"}], "}"}]}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"z", " ", "d"}], " ", "\[Equal]", "1"}], ",", " ", 
     RowBox[{
      RowBox[{"d", " ", "x"}], " ", "\[Equal]", " ", "uL"}]}], "}"}]}], 
  "]"}]], "Input",
 CellChangeTimes->{{3.534982548021124*^9, 3.534982548331835*^9}, {
   3.534985672071082*^9, 3.5349856752309647`*^9}, {3.53498575691752*^9, 
   3.534985773748554*^9}, {3.534985930341876*^9, 3.534985932316481*^9}, {
   3.534986318557267*^9, 3.534986330749145*^9}, 3.534986878092679*^9, {
   3.535027890326747*^9, 3.5350279684574137`*^9}, {3.535029655571067*^9, 
   3.5350296880567923`*^9}, 3.535029764887192*^9, {3.535030004090427*^9, 
   3.535030121269846*^9}, {3.535030273225251*^9, 3.5350302878805017`*^9}, {
   3.535030326279409*^9, 3.535030326575099*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"uL", " ", "v"}], 
      RowBox[{
       RowBox[{"-", "1"}], "-", 
       SuperscriptBox["uL", "2"]}], "v", 
      RowBox[{"-", "d"}], "0", 
      RowBox[{"d", " ", "uL"}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R00", "-", 
         RowBox[{"R02", " ", "uL"}]}], ")"}]}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R10", "-", 
         RowBox[{"R12", " ", "uL"}]}], ")"}]}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R20", "-", 
         RowBox[{"R22", " ", "uL"}]}], ")"}]}]},
     {
      RowBox[{"uR", " ", "v"}], 
      RowBox[{
       RowBox[{"-", "1"}], "-", 
       RowBox[{"uL", " ", "uR"}]}], "v", 
      RowBox[{"-", "d"}], "0", 
      RowBox[{"d", " ", "uR"}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R00", "-", 
         RowBox[{"R02", " ", "uR"}]}], ")"}]}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R10", "-", 
         RowBox[{"R12", " ", "uR"}]}], ")"}]}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R20", "-", 
         RowBox[{"R22", " ", "uR"}]}], ")"}]}]},
     {
      RowBox[{"1", "+", 
       SuperscriptBox["v", "2"]}], 
      RowBox[{
       RowBox[{"-", "uL"}], " ", "v"}], 
      RowBox[{"-", "uL"}], "0", 
      RowBox[{"-", "d"}], 
      RowBox[{"d", " ", "v"}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R01", "-", 
         RowBox[{"R02", " ", "v"}]}], ")"}]}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R11", "-", 
         RowBox[{"R12", " ", "v"}]}], ")"}]}], 
      RowBox[{"d", " ", 
       RowBox[{"(", 
        RowBox[{"R21", "-", 
         RowBox[{"R22", " ", "v"}]}], ")"}]}]}
    },
    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.535030258510765*^9, 3.535030288430678*^9}, 
   3.535030327829464*^9, 3.535031349766954*^9}]
}, Open  ]]
},
WindowSize->{738, 817},
WindowMargins->{{Automatic, 217}, {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, 239, 5, 26, "Text"],
Cell[CellGroupData[{
Cell[821, 29, 1945, 44, 61, "Input"],
Cell[2769, 75, 3751, 119, 109, "Output"]
}, Open  ]],
Cell[6535, 197, 201, 4, 26, "Text"],
Cell[CellGroupData[{
Cell[6761, 205, 2402, 54, 80, "Input"],
Cell[9166, 261, 3050, 84, 99, "Output"]
}, Open  ]],
Cell[12231, 348, 107, 1, 26, "Text"],
Cell[CellGroupData[{
Cell[12363, 353, 950, 21, 27, "Input"],
Cell[13316, 376, 2384, 75, 79, "Output"]
}, Open  ]]
}
]
*)

(* End of internal cache information *)