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_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
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Subsection
|
\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;
|
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) {
|
Matrix6 Pose3::adjointMap(const Vector6& xi) {
|
||||||
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
|
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
|
* 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)
|
* 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$
|
* \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 {
|
Vector6 Adjoint(const Vector6& xi_b,
|
||||||
return AdjointMap() * xi_b;
|
OptionalJacobian<6, 6> H_this = boost::none,
|
||||||
} /// FIXME Not tested - marked as incorrect
|
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
|
* 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));
|
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))
|
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
|
||||||
TEST(Pose3, Adjoint_hat)
|
TEST(Pose3, Adjoint_hat)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue