diff --git a/doc/math.lyx b/doc/math.lyx index f14ebc66f..6d7a5e318 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5090,29 +5090,22 @@ Derivative of Adjoint \begin_layout Standard Consider -\begin_inset Formula $f:SE(3)\rightarrow\mathbb{R}^{6}$ +\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ \end_inset is defined as -\begin_inset Formula $f(T)=Ad_{T}y$ -\end_inset - - for some constant -\begin_inset Formula $y=\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}$ +\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$ \end_inset . - Defining -\begin_inset Formula $\xi=\begin{bmatrix}\omega\\ -v -\end{bmatrix}$ -\end_inset + The derivative is notated (see +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" - for the derivative notation (w.r.t. - pose -\begin_inset Formula $T$ \end_inset ): @@ -5121,68 +5114,17 @@ v \begin_layout Standard \begin_inset Formula \[ -f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix} +Df_{(T,y)}(\xi,\delta y)=D_{1}f_{(T,y)}(\xi)+D_{2}f_{(T,y)}(\delta y) \] \end_inset -Then we can compute -\begin_inset Formula $f'(T)$ +First, computing +\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$ \end_inset - by considering the rotation and translation separately. - To reduce confusion with -\begin_inset Formula $\omega_{y},v_{y}$ -\end_inset - -, we will use -\begin_inset Formula $R,t$ -\end_inset - - to denote the rotation and translation of -\begin_inset Formula $T\exp\xi$ -\end_inset - -. -\end_layout - -\begin_layout Standard -\begin_inset Formula -\[ -\frac{\partial}{\partial\omega}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}=\frac{\partial}{\partial\omega}\begin{bmatrix}R\omega_{y}\\{} -[t]_{\times}R\omega_{y}+Rv_{y} -\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times}\\ --[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} -\end{bmatrix} -\] - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Formula -\[ -\frac{\partial}{\partial t}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega\\ -v -\end{bmatrix}=\frac{\partial}{\partial t}\begin{bmatrix}R\omega_{y}\\{} -[t]_{\times}R\omega_{y}+Rv_{y} -\end{bmatrix}=\begin{bmatrix}0\\ --[R\omega_{y}] -\end{bmatrix} -\] - -\end_inset - -Applying chain rule for the translation, -\begin_inset Formula $\frac{\partial}{\partial v}=\frac{\partial}{\partial t}\frac{\partial t}{\partial v}$ + is easy, as its matrix is simply +\begin_inset Formula $Ad_{T}$ \end_inset : @@ -5191,76 +5133,139 @@ Applying chain rule for the translation, \begin_layout Standard \begin_inset Formula \[ -\frac{\partial}{\partial v}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}=\begin{bmatrix}0\\ --[R\omega_{y}]_{\times} -\end{bmatrix}R=\begin{bmatrix}0\\ --[R\omega_{y}]_{\times}R -\end{bmatrix}=\begin{bmatrix}0\\ --R[\omega_{y}]_{\times} -\end{bmatrix} +f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b}) \] \end_inset -The simplification -\family roman -\series medium -\shape up -\size normal -\emph off -\bar no -\strikeout off -\xout off -\uuline off -\uwave off -\noun off -\color none -\begin_inset Formula $[R\omega_{y}]_{\times}R=R[\omega_{y}]_{\times}$ -\end_inset - - can be derived by considering the result for when -\begin_inset Formula $\omega_{y}$ -\end_inset - - is each of the 3 standard basis vectors -\begin_inset Formula $\hat{e}_{i}$ -\end_inset - -: -\begin_inset Formula $-[R\hat{e}_{i}]_{\times}R$ -\end_inset - -. -\end_layout - -\begin_layout Standard -Now putting together the rotation and translation: \end_layout \begin_layout Standard \begin_inset Formula \[ -f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times} & 0\\ --[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} & -R[\omega_{y}]_{\times} -\end{bmatrix} +D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} \] \end_inset +To compute +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset -\end_layout - -\begin_layout Standard -We can apply a similar procedure to compute the derivative of -\begin_inset Formula $Ad_{T}^{T}y$ +, we'll first define +\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ \end_inset . + From Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" + +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\ +D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1} +\end{align*} + +\end_inset + +Now we can use the definition of the Adjoint representation +\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$ +\end_inset + + (aka conjugation by +\begin_inset Formula $g$ +\end_inset + +) then apply product rule and simplify: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}g^{-1}\right)(\xi)\\ + & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}g^{-1}(T,0)+g(T,0)\hat{\xi}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ + & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ + & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ + & =-Ad_{T}(ad_{\xi_{b}}\hat{\xi})\\ +D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) +\end{align*} + +\end_inset + +An alternative, perhaps more intuitive way of deriving this would be to + use the fact that the derivative at the origin +\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ +\end_inset + + by definition of the adjoint +\begin_inset Formula $ad_{\xi}$ +\end_inset + +. + Then applying the property +\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$ +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}_{b}}(\xi)\right) +\] + +\end_inset + +It's difficult to apply a similar procedure to compute the derivative of + +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + (note the +\begin_inset Formula $^{*}$ +\end_inset + + denoting that we are now in the dual space) because +\begin_inset Formula $Ad_{T}^{T}$ +\end_inset + + cannot be naturally defined as a conjugation so we resort to crunching + through the algebra. + The details are omitted but the result is a form vaguely resembling (but + not quite) the +\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +\begin{bmatrix}\omega_{T}\\ +v_{T} +\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\ +D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\ +\hat{v}_{T} & 0 +\end{bmatrix} +\end{align*} + +\end_inset + + \end_layout \begin_layout Subsection diff --git a/doc/math.pdf b/doc/math.pdf index 06dec078d..71f9dadc6 100644 Binary files a/doc/math.pdf and b/doc/math.pdf differ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ed605d8b6..4bfb574b1 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -67,88 +67,41 @@ Matrix6 Pose3::AdjointMap() const { // Calculate AdjointMap applied to xi_b, with Jacobians Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_xib) const { - // Ad * xi = [ R 0 . [w - // [t]R R ] v] - // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; // = AdjointMap() * xi - auto Rw = result.head<3>(); - const auto &w = xi_b.head<3>(), &v = xi_b.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R, pRw_H_Rw; - const Matrix3 R = R_.matrix(); - const Matrix3 &Rw_H_w = R; - const Matrix3 &Rv_H_v = R; - - // Calculations - Rw = R_.rotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); - const Vector3 Rv = R_.rotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 pRw = cross(t_, Rw, boost::none /* pRw_H_t */, pRw_H_Rw); - result.tail<3>() = pRw + Rv; + const Matrix6 Ad = AdjointMap(); // Jacobians - if (H_pose) { - // pRw_H_posev = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R - // where [ ]x denotes the skew-symmetric operator. - // See docs/math.pdf for more details. - const Matrix3 &pRw_H_posev = Rw_H_R; - *H_pose = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_posev) - .finished(); - } - if (H_xib) { - // This is just equal to AdjointMap() but we can reuse pRw_H_Rw = [t]x - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, - /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) - .finished(); - } + // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b + // D2 Ad_T(xi_b) = Ad_T + // See docs/math.pdf for more details. + // In D1 calculation, we could be more efficient by writing it out, but do not + // for readability + if (H_pose) *H_pose = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; - // Return - we computed result manually but it should be = AdjointMap() * xi - return result; + return Ad * xi_b; } /* ************************************************************************* */ /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { - // Ad^T * xi = [ R^T R^T.[-t] . [w - // 0 R^T ] v] - // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; // = AdjointMap().transpose() * x - const Vector3 &w = x.head<3>(), &v = x.tail<3>(); - auto Rv = result.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; - const Matrix3 Rtranspose = R_.matrix().transpose(); - const Matrix3 &Rw_H_w = Rtranspose; - const Matrix3 &Rv_H_v = Rtranspose; - const Matrix3 &Rtv_H_tv = Rtranspose; - const Matrix3 tv_H_v = skewSymmetric(t_); - - // Calculations - const Vector3 Rw = R_.unrotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); - Rv = R_.unrotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 tv = cross(t_, v, boost::none /* tv_H_t */, tv_H_v); - const Vector3 Rtv = - R_.unrotate(tv, H_pose ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); - result.head<3>() = Rw - Rtv; + const Matrix6 &AdT = AdjointMap().transpose(); + const Vector6 &AdTx = AdT * x; // Jacobians + // See docs/math.pdf for more details. if (H_pose) { - // Rtv_H_posev = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R - // where [ ]x denotes the skew-symmetric operator. - // See docs/math.pdf for more details. - const Matrix3 &Rtv_H_posev = Rv_H_R; - *H_pose = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_posev, - /* */ Rv_H_R, /* */ Z_3x3) + const auto &w_T_hat = skewSymmetric(AdTx.head<3>()), + &v_T_hat = skewSymmetric(AdTx.tail<3>()); + *H_pose = (Matrix6() << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3) .finished(); } if (H_x) { - // This is just equal to AdjointMap().transpose() but we can reuse [t]x - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, - /* */ Z_3x3, Rv_H_v) - .finished(); + *H_x = AdT; } - // Return - this should be equivalent to AdjointMap().transpose() * xi - return result; + return AdTx; } /* ************************************************************************* */