much cleaner Adjoint jacobian
Gerry Chen
parent
31e5cbb81a
commit
f0fc0d4457
253
doc/math.lyx
253
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
|
||||
|
|
|
|||
BIN
doc/math.pdf
BIN
doc/math.pdf
Binary file not shown.
|
|
@ -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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue