Updated BetweenFactor section of 'math' document
							parent
							
								
									d4ec018d0f
								
							
						
					
					
						commit
						8f4c1278b9
					
				
							
								
								
									
										205
									
								
								doc/math.lyx
								
								
								
								
							
							
						
						
									
										205
									
								
								doc/math.lyx
								
								
								
								
							|  | @ -885,7 +885,39 @@ H_{a}=G_{f(a)}F_{a} | |||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| where  | ||||
| \begin_inset Formula $G_{f(a)}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the  | ||||
| \begin_inset Formula $m\times p$ | ||||
| \end_inset | ||||
| 
 | ||||
|  Jacobian matrix of  | ||||
| \begin_inset Formula $g$ | ||||
| \end_inset | ||||
| 
 | ||||
|  evaluated at  | ||||
| \begin_inset Formula $f(a)$ | ||||
| \end_inset | ||||
| 
 | ||||
| , and  | ||||
| \begin_inset Formula $F_{a}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the  | ||||
| \begin_inset Formula $p\times n$ | ||||
| \end_inset | ||||
| 
 | ||||
|  Jacobian matrix of  | ||||
| \begin_inset Formula $f$ | ||||
| \end_inset | ||||
| 
 | ||||
|  evaluated at  | ||||
| \begin_inset Formula $a$ | ||||
| \end_inset | ||||
| 
 | ||||
| . | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Proof | ||||
|  | @ -1521,7 +1553,7 @@ name "th:Action" | |||
| \end_inset | ||||
| 
 | ||||
| The Jacobian matrix of the group action | ||||
| \begin_inset Formula $f(T,P)=Tp$ | ||||
| \begin_inset Formula $f(T,p)=Tp$ | ||||
| \end_inset | ||||
| 
 | ||||
|  at  | ||||
|  | @ -2811,6 +2843,12 @@ B^{T} & I_{3}\end{array}\right] | |||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Subsection | ||||
| \begin_inset CommandInset label | ||||
| LatexCommand label | ||||
| name "sub:Pushforward-of-Between" | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| Pushforward of Between | ||||
| \end_layout | ||||
| 
 | ||||
|  | @ -3378,27 +3416,14 @@ BetweenFactor | |||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| BetweenFactor is often used to summarize  | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| Theorem  | ||||
| \begin_inset CommandInset ref | ||||
| LatexCommand ref | ||||
| reference "D-exp" | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
|  about the derivative of the exponential map  | ||||
| \begin_inset Formula $f:\xi\mapsto\exp\xihat$ | ||||
| \end_inset | ||||
| 
 | ||||
|  being identity only at  | ||||
| \begin_inset Formula $\xi=0$ | ||||
| \end_inset | ||||
| 
 | ||||
|  has implications for GTSAM. | ||||
|  Given two elements  | ||||
| \series bold | ||||
| \emph on | ||||
| BetweenFactor | ||||
| \series default | ||||
| \emph default | ||||
|  is a factor in GTSAM that is used ubiquitously to process measurements | ||||
|  indicating the relative pose between two unknown poses  | ||||
| \begin_inset Formula $T_{1}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  | @ -3406,15 +3431,104 @@ reference "D-exp" | |||
| \begin_inset Formula $T_{2}$ | ||||
| \end_inset | ||||
| 
 | ||||
| , BetweenFactor evaluates | ||||
| . | ||||
|  Let us assume the measured relative pose is  | ||||
| \begin_inset Formula $Z$ | ||||
| \end_inset | ||||
| 
 | ||||
| , then the code that calculates the error in  | ||||
| \series bold | ||||
| \emph on | ||||
| BetweenFactor | ||||
| \series default | ||||
| \emph default | ||||
|  first calculates the predicted relative pose  | ||||
| \begin_inset Formula $T_{12}$ | ||||
| \end_inset | ||||
| 
 | ||||
| , and then evaluates the error between the measured and predicted relative | ||||
|  pose: | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout LyX-Code | ||||
| T12 = between(T1, T2); | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout LyX-Code | ||||
| return localCoordinates(Z, T12); | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| where we recall that the function  | ||||
| \series bold | ||||
| \emph on | ||||
| between | ||||
| \series default | ||||
| \emph default | ||||
|  is given in group theoretic notation as  | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| g(T_{1},T_{2};Z)=f^{-1}\left(\mathop{between}(Z,\mathop{between}(T_{1},T_{2})\right)=f^{-1}\left(Z^{-1}\left(T_{1}^{-1}T_{2}\right)\right) | ||||
| \varphi(g,h)=g^{-1}h | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| but because it is assumed that  | ||||
| The function  | ||||
| \series bold | ||||
| \emph on | ||||
| localCoordinates | ||||
| \series default | ||||
| \emph default | ||||
|  itself also calls  | ||||
| \series bold | ||||
| \emph on | ||||
| between | ||||
| \series default | ||||
| \emph default | ||||
| , and converts to canonical coordinates:  | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout LyX-Code | ||||
| localCoordinates(Z,T12) = Logmap(between(Z, T12)); | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| Hence, given two elements  | ||||
| \begin_inset Formula $T_{1}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  and  | ||||
| \begin_inset Formula $T_{2}$ | ||||
| \end_inset | ||||
| 
 | ||||
| ,  | ||||
| \series bold | ||||
| \emph on | ||||
| BetweenFactor | ||||
| \series default | ||||
| \emph default | ||||
|  evaluates  | ||||
| \begin_inset Formula $g:G\times G\rightarrow\Reals n$ | ||||
| \end_inset | ||||
| 
 | ||||
| , | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| g(T_{1},T_{2};Z)=f^{-1}\left(\varphi(Z,\varphi(T_{1},T_{2})\right)=f^{-1}\left(Z^{-1}\left(T_{1}^{-1}T_{2}\right)\right) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| where  | ||||
| \begin_inset Formula $f^{-1}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the inverse of the map  | ||||
| \begin_inset Formula $f:\xi\mapsto\exp\xihat$ | ||||
| \end_inset | ||||
| 
 | ||||
| . | ||||
|  If we assume that the measurement has only small error, then  | ||||
| \begin_inset Formula $Z\approx T_{1}^{-1}T_{2}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  | @ -3422,12 +3536,49 @@ but because it is assumed that | |||
| \begin_inset Formula $Z^{-1}T_{1}^{-1}T_{2}\approx e$ | ||||
| \end_inset | ||||
| 
 | ||||
|  and the derivative should be good there. | ||||
|  Note that the derivative of  | ||||
| , and we can invoke Theorem  | ||||
| \begin_inset CommandInset ref | ||||
| LatexCommand ref | ||||
| reference "D-exp" | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| , which says that the derivative of the exponential map  | ||||
| \begin_inset Formula $f:\xi\mapsto\exp\xihat$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is identity at  | ||||
| \begin_inset Formula $\xi=0$ | ||||
| \end_inset | ||||
| 
 | ||||
| , as well as its inverse. | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| Finally, because the derivative of  | ||||
| \series bold | ||||
| \emph on | ||||
| between | ||||
| \series default | ||||
| \emph default | ||||
|  is identity in its second argument. | ||||
|  is identity in its second argument, the derivative of the  | ||||
| \series bold | ||||
| \emph on | ||||
| BetweenFactor | ||||
| \series default | ||||
| \emph default | ||||
|  error is identical to the derivative of pushforward of  | ||||
| \begin_inset Formula $\varphi(T_{1},T_{2})$ | ||||
| \end_inset | ||||
| 
 | ||||
| , derived in Section  | ||||
| \begin_inset CommandInset ref | ||||
| LatexCommand ref | ||||
| reference "sub:Pushforward-of-Between" | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| . | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Section | ||||
|  |  | |||
							
								
								
									
										
											BIN
										
									
								
								doc/math.pdf
								
								
								
								
							
							
						
						
									
										
											BIN
										
									
								
								doc/math.pdf
								
								
								
								
							
										
											Binary file not shown.
										
									
								
							
		Loading…
	
		Reference in New Issue