Merge pull request #885 from borglab/feature/Pose3AdjointMapJacobians
`Pose3::Adjoint(xi)` Jacobiansrelease/4.3a0
Gerry Chen
GitHub
commit
7b22090352
388
doc/math.lyx
388
doc/math.lyx
|
|
@ -5082,6 +5082,394 @@ reference "ex:projection"
|
|||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsection
|
||||
Derivative of Adjoint
|
||||
\begin_inset CommandInset label
|
||||
LatexCommand label
|
||||
name "subsec:pose3_adjoint_deriv"
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Consider
|
||||
\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$
|
||||
\end_inset
|
||||
|
||||
is defined as
|
||||
\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
The derivative is notated (see 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
|
||||
\[
|
||||
Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b})
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
First, computing
|
||||
\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$
|
||||
\end_inset
|
||||
|
||||
is easy, as its matrix is simply
|
||||
\begin_inset Formula $Ad_{T}$
|
||||
\end_inset
|
||||
|
||||
:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
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
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
We will derive
|
||||
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$
|
||||
\end_inset
|
||||
|
||||
using two approaches.
|
||||
In the first, we'll 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}_{b}g^{-1}\right)(\xi)\\
|
||||
& =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\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_{\hat{\xi}}\hat{\xi}_{b})\\
|
||||
& =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\
|
||||
D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}})
|
||||
\end{align*}
|
||||
|
||||
\end_inset
|
||||
|
||||
Where
|
||||
\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$
|
||||
\end_inset
|
||||
|
||||
is the adjoint map of the lie algebra.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The second, perhaps more intuitive way of deriving
|
||||
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$
|
||||
\end_inset
|
||||
|
||||
, 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}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsection
|
||||
Derivative of AdjointTranspose
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The transpose of the Adjoint,
|
||||
\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 $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$
|
||||
\end_inset
|
||||
|
||||
, is useful as a way to change the reference frame of vectors in the dual
|
||||
space
|
||||
\family default
|
||||
\series default
|
||||
\shape default
|
||||
\size default
|
||||
\emph default
|
||||
\bar default
|
||||
\strikeout default
|
||||
\xout default
|
||||
\uuline default
|
||||
\uwave default
|
||||
\noun default
|
||||
\color inherit
|
||||
(note the
|
||||
\begin_inset Formula $^{*}$
|
||||
\end_inset
|
||||
|
||||
denoting that we are now in the dual space)
|
||||
\family roman
|
||||
\series medium
|
||||
\shape up
|
||||
\size normal
|
||||
\emph off
|
||||
\bar no
|
||||
\strikeout off
|
||||
\xout off
|
||||
\uuline off
|
||||
\uwave off
|
||||
\noun off
|
||||
\color none
|
||||
.
|
||||
To be more concrete, where
|
||||
\family default
|
||||
\series default
|
||||
\shape default
|
||||
\size default
|
||||
\emph default
|
||||
\bar default
|
||||
\strikeout default
|
||||
\xout default
|
||||
\uuline default
|
||||
\uwave default
|
||||
\noun default
|
||||
\color inherit
|
||||
as
|
||||
\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$
|
||||
\end_inset
|
||||
|
||||
converts the
|
||||
\emph on
|
||||
twist
|
||||
\emph default
|
||||
|
||||
\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 $\xi_{b}$
|
||||
\end_inset
|
||||
|
||||
from the
|
||||
\begin_inset Formula $T$
|
||||
\end_inset
|
||||
|
||||
frame,
|
||||
\family default
|
||||
\series default
|
||||
\shape default
|
||||
\size default
|
||||
\emph default
|
||||
\bar default
|
||||
\strikeout default
|
||||
\xout default
|
||||
\uuline default
|
||||
\uwave default
|
||||
\noun default
|
||||
\color inherit
|
||||
|
||||
\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 $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
|
||||
\end_inset
|
||||
|
||||
converts the
|
||||
\family default
|
||||
\series default
|
||||
\shape default
|
||||
\size default
|
||||
\emph on
|
||||
\bar default
|
||||
\strikeout default
|
||||
\xout default
|
||||
\uuline default
|
||||
\uwave default
|
||||
\noun default
|
||||
\color inherit
|
||||
wrench
|
||||
\emph default
|
||||
|
||||
\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 $\xi_{b}^{*}$
|
||||
\end_inset
|
||||
|
||||
from the
|
||||
\begin_inset Formula $T$
|
||||
\end_inset
|
||||
|
||||
frame
|
||||
\family default
|
||||
\series default
|
||||
\shape default
|
||||
\size default
|
||||
\emph default
|
||||
\bar default
|
||||
\strikeout default
|
||||
\xout default
|
||||
\uuline default
|
||||
\uwave default
|
||||
\noun default
|
||||
\color inherit
|
||||
.
|
||||
It's difficult to apply a similar derivation as in Section
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand ref
|
||||
reference "subsec:pose3_adjoint_deriv"
|
||||
plural "false"
|
||||
caps "false"
|
||||
noprefix "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
for the derivative of
|
||||
\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
|
||||
\end_inset
|
||||
|
||||
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 that vaguely resembles
|
||||
(but does not exactly match)
|
||||
\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.
|
|
@ -63,6 +63,47 @@ Matrix6 Pose3::AdjointMap() const {
|
|||
return adj;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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 {
|
||||
const Matrix6 Ad = AdjointMap();
|
||||
|
||||
// Jacobians
|
||||
// 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 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 {
|
||||
const Matrix6 &AdT = AdjointMap().transpose();
|
||||
const Vector6 &AdTx = AdT * x;
|
||||
|
||||
// Jacobians
|
||||
// See docs/math.pdf for more details.
|
||||
if (H_pose) {
|
||||
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) {
|
||||
*H_x = AdT;
|
||||
}
|
||||
|
||||
return AdTx;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::adjointMap(const Vector6& xi) {
|
||||
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
|
||||
|
|
|
|||
|
|
@ -145,15 +145,22 @@ public:
|
|||
* Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
|
||||
* Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
|
||||
*/
|
||||
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
|
||||
Matrix6 AdjointMap() const;
|
||||
|
||||
/**
|
||||
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
|
||||
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
|
||||
* body-fixed velocity, transforming it to the spatial frame
|
||||
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
|
||||
* Note that H_xib = AdjointMap()
|
||||
*/
|
||||
Vector6 Adjoint(const Vector6& xi_b) const {
|
||||
return AdjointMap() * xi_b;
|
||||
} /// FIXME Not tested - marked as incorrect
|
||||
Vector6 Adjoint(const Vector6& xi_b,
|
||||
OptionalJacobian<6, 6> H_this = boost::none,
|
||||
OptionalJacobian<6, 6> H_xib = boost::none) const;
|
||||
|
||||
/// The dual version of Adjoint
|
||||
Vector6 AdjointTranspose(const Vector6& x,
|
||||
OptionalJacobian<6, 6> H_this = boost::none,
|
||||
OptionalJacobian<6, 6> H_x = boost::none) const;
|
||||
|
||||
/**
|
||||
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
|
||||
|
|
|
|||
|
|
@ -145,6 +145,81 @@ TEST(Pose3, Adjoint_full)
|
|||
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check Adjoint numerical derivatives
|
||||
TEST(Pose3, Adjoint_jacobians)
|
||||
{
|
||||
Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
|
||||
|
||||
// Check evaluation sanity check
|
||||
EQUALITY(static_cast<gtsam::Vector>(T.AdjointMap() * xi), T.Adjoint(xi));
|
||||
EQUALITY(static_cast<gtsam::Vector>(T2.AdjointMap() * xi), T2.Adjoint(xi));
|
||||
EQUALITY(static_cast<gtsam::Vector>(T3.AdjointMap() * xi), T3.Adjoint(xi));
|
||||
|
||||
// Check jacobians
|
||||
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
|
||||
std::function<Vector6(const Pose3&, const Vector6&)> Adjoint_proxy =
|
||||
[&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); };
|
||||
|
||||
T.Adjoint(xi, actualH1, actualH2);
|
||||
expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi);
|
||||
expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi);
|
||||
EXPECT(assert_equal(expectedH1, actualH1));
|
||||
EXPECT(assert_equal(expectedH2, actualH2));
|
||||
|
||||
T2.Adjoint(xi, actualH1, actualH2);
|
||||
expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi);
|
||||
expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi);
|
||||
EXPECT(assert_equal(expectedH1, actualH1));
|
||||
EXPECT(assert_equal(expectedH2, actualH2));
|
||||
|
||||
T3.Adjoint(xi, actualH1, actualH2);
|
||||
expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi);
|
||||
expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi);
|
||||
EXPECT(assert_equal(expectedH1, actualH1));
|
||||
EXPECT(assert_equal(expectedH2, actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check AdjointTranspose and jacobians
|
||||
TEST(Pose3, AdjointTranspose)
|
||||
{
|
||||
Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
|
||||
|
||||
// Check evaluation
|
||||
EQUALITY(static_cast<Vector>(T.AdjointMap().transpose() * xi),
|
||||
T.AdjointTranspose(xi));
|
||||
EQUALITY(static_cast<Vector>(T2.AdjointMap().transpose() * xi),
|
||||
T2.AdjointTranspose(xi));
|
||||
EQUALITY(static_cast<Vector>(T3.AdjointMap().transpose() * xi),
|
||||
T3.AdjointTranspose(xi));
|
||||
|
||||
// Check jacobians
|
||||
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
|
||||
std::function<Vector6(const Pose3&, const Vector6&)> AdjointTranspose_proxy =
|
||||
[&](const Pose3& T, const Vector6& xi) {
|
||||
return T.AdjointTranspose(xi);
|
||||
};
|
||||
|
||||
T.AdjointTranspose(xi, actualH1, actualH2);
|
||||
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi);
|
||||
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);
|
||||
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
||||
EXPECT(assert_equal(expectedH2, actualH2));
|
||||
|
||||
T2.AdjointTranspose(xi, actualH1, actualH2);
|
||||
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi);
|
||||
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi);
|
||||
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
||||
EXPECT(assert_equal(expectedH2, actualH2));
|
||||
|
||||
T3.AdjointTranspose(xi, actualH1, actualH2);
|
||||
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi);
|
||||
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi);
|
||||
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
||||
EXPECT(assert_equal(expectedH2, actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
|
||||
TEST(Pose3, Adjoint_hat)
|
||||
|
|
|
|||
Loading…
Reference in New Issue