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
|
\begin_layout Standard
|
||||||
Consider
|
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
|
\end_inset
|
||||||
|
|
||||||
is defined as
|
is defined as
|
||||||
\begin_inset Formula $f(T)=Ad_{T}y$
|
\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$
|
||||||
\end_inset
|
|
||||||
|
|
||||||
for some constant
|
|
||||||
\begin_inset Formula $y=\begin{bmatrix}\omega_{y}\\
|
|
||||||
v_{y}
|
|
||||||
\end{bmatrix}$
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
.
|
.
|
||||||
Defining
|
The derivative is notated (see
|
||||||
\begin_inset Formula $\xi=\begin{bmatrix}\omega\\
|
\begin_inset CommandInset ref
|
||||||
v
|
LatexCommand ref
|
||||||
\end{bmatrix}$
|
reference "sec:Derivatives-of-Actions"
|
||||||
\end_inset
|
plural "false"
|
||||||
|
caps "false"
|
||||||
|
noprefix "false"
|
||||||
|
|
||||||
for the derivative notation (w.r.t.
|
|
||||||
pose
|
|
||||||
\begin_inset Formula $T$
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
):
|
):
|
||||||
|
|
@ -5121,68 +5114,17 @@ v
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
\begin_inset Formula
|
\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
|
\end_inset
|
||||||
|
|
||||||
Then we can compute
|
First, computing
|
||||||
\begin_inset Formula $f'(T)$
|
\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
by considering the rotation and translation separately.
|
is easy, as its matrix is simply
|
||||||
To reduce confusion with
|
\begin_inset Formula $Ad_{T}$
|
||||||
\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}$
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
:
|
:
|
||||||
|
|
@ -5191,76 +5133,139 @@ Applying chain rule for the translation,
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
\begin_inset Formula
|
\begin_inset Formula
|
||||||
\[
|
\[
|
||||||
\frac{\partial}{\partial v}\begin{bmatrix}R & 0\\{}
|
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})
|
||||||
[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}
|
|
||||||
\]
|
\]
|
||||||
|
|
||||||
\end_inset
|
\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
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
\begin_inset Formula
|
\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\\
|
D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T}
|
||||||
-[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} & -R[\omega_{y}]_{\times}
|
|
||||||
\end{bmatrix}
|
|
||||||
\]
|
\]
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
To compute
|
||||||
|
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
\end_layout
|
, we'll first define
|
||||||
|
\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$
|
||||||
\begin_layout Standard
|
|
||||||
We can apply a similar procedure to compute the derivative of
|
|
||||||
\begin_inset Formula $Ad_{T}^{T}y$
|
|
||||||
\end_inset
|
\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
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Subsection
|
\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
|
// Calculate AdjointMap applied to xi_b, with Jacobians
|
||||||
Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
|
Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
|
||||||
OptionalJacobian<6, 6> H_xib) const {
|
OptionalJacobian<6, 6> H_xib) const {
|
||||||
// Ad * xi = [ R 0 . [w
|
const Matrix6 Ad = AdjointMap();
|
||||||
// [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;
|
|
||||||
|
|
||||||
// Jacobians
|
// Jacobians
|
||||||
if (H_pose) {
|
// D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b
|
||||||
// pRw_H_posev = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R
|
// D2 Ad_T(xi_b) = Ad_T
|
||||||
// where [ ]x denotes the skew-symmetric operator.
|
// See docs/math.pdf for more details.
|
||||||
// See docs/math.pdf for more details.
|
// In D1 calculation, we could be more efficient by writing it out, but do not
|
||||||
const Matrix3 &pRw_H_posev = Rw_H_R;
|
// for readability
|
||||||
*H_pose = (Matrix6() << Rw_H_R, /* */ Z_3x3, //
|
if (H_pose) *H_pose = -Ad * adjointMap(xi_b);
|
||||||
/* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_posev)
|
if (H_xib) *H_xib = Ad;
|
||||||
.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();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return - we computed result manually but it should be = AdjointMap() * xi
|
return Ad * xi_b;
|
||||||
return result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/// The dual version of Adjoint
|
/// The dual version of Adjoint
|
||||||
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
|
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
|
||||||
OptionalJacobian<6, 6> H_x) const {
|
OptionalJacobian<6, 6> H_x) const {
|
||||||
// Ad^T * xi = [ R^T R^T.[-t] . [w
|
const Matrix6 &AdT = AdjointMap().transpose();
|
||||||
// 0 R^T ] v]
|
const Vector6 &AdTx = AdT * x;
|
||||||
// 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;
|
|
||||||
|
|
||||||
// Jacobians
|
// Jacobians
|
||||||
|
// See docs/math.pdf for more details.
|
||||||
if (H_pose) {
|
if (H_pose) {
|
||||||
// Rtv_H_posev = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R
|
const auto &w_T_hat = skewSymmetric(AdTx.head<3>()),
|
||||||
// where [ ]x denotes the skew-symmetric operator.
|
&v_T_hat = skewSymmetric(AdTx.tail<3>());
|
||||||
// See docs/math.pdf for more details.
|
*H_pose = (Matrix6() << w_T_hat, v_T_hat, //
|
||||||
const Matrix3 &Rtv_H_posev = Rv_H_R;
|
/* */ v_T_hat, Z_3x3)
|
||||||
*H_pose = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_posev,
|
|
||||||
/* */ Rv_H_R, /* */ Z_3x3)
|
|
||||||
.finished();
|
.finished();
|
||||||
}
|
}
|
||||||
if (H_x) {
|
if (H_x) {
|
||||||
// This is just equal to AdjointMap().transpose() but we can reuse [t]x
|
*H_x = AdT;
|
||||||
*H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v,
|
|
||||||
/* */ Z_3x3, Rv_H_v)
|
|
||||||
.finished();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return - this should be equivalent to AdjointMap().transpose() * xi
|
return AdTx;
|
||||||
return result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue