Frank Dellaert
commit
9e4b4b3f72
|
|
@ -1,5 +1,5 @@
|
|||
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 413
|
||||
#LyX 2.1 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 474
|
||||
\begin_document
|
||||
\begin_header
|
||||
\textclass article
|
||||
|
|
@ -15,13 +15,13 @@ theorems-std
|
|||
\font_roman times
|
||||
\font_sans default
|
||||
\font_typewriter default
|
||||
\font_math auto
|
||||
\font_default_family rmdefault
|
||||
\use_non_tex_fonts false
|
||||
\font_sc false
|
||||
\font_osf false
|
||||
\font_sf_scale 100
|
||||
\font_tt_scale 100
|
||||
|
||||
\graphics default
|
||||
\default_output_format default
|
||||
\output_sync 0
|
||||
|
|
@ -32,15 +32,24 @@ theorems-std
|
|||
\use_hyperref false
|
||||
\papersize default
|
||||
\use_geometry true
|
||||
\use_amsmath 1
|
||||
\use_esint 0
|
||||
\use_mhchem 1
|
||||
\use_mathdots 1
|
||||
\use_package amsmath 1
|
||||
\use_package amssymb 1
|
||||
\use_package cancel 1
|
||||
\use_package esint 0
|
||||
\use_package mathdots 1
|
||||
\use_package mathtools 1
|
||||
\use_package mhchem 1
|
||||
\use_package stackrel 1
|
||||
\use_package stmaryrd 1
|
||||
\use_package undertilde 1
|
||||
\cite_engine basic
|
||||
\cite_engine_type default
|
||||
\biblio_style plain
|
||||
\use_bibtopic false
|
||||
\use_indices false
|
||||
\paperorientation portrait
|
||||
\suppress_date false
|
||||
\justification true
|
||||
\use_refstyle 0
|
||||
\index Index
|
||||
\shortcut idx
|
||||
|
|
@ -96,7 +105,7 @@ We will start with a small example of a robot moving in a plane, parameterized
|
|||
2D pose
|
||||
\emph default
|
||||
|
||||
\begin_inset Formula $(x,\, y,\,\theta)$
|
||||
\begin_inset Formula $(x,\,y,\,\theta)$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
|
|
@ -135,7 +144,7 @@ A similar story holds for translation in the
|
|||
direction, and in fact for translations in general:
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0})
|
||||
(x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0})
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -143,7 +152,7 @@ A similar story holds for translation in the
|
|||
Similarly for rotation we have
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(x_{t},\, y_{t},\,\theta_{t})=(x_{0},\, y_{0},\,\theta_{0}+\omega t)
|
||||
(x_{t},\,y_{t},\,\theta_{t})=(x_{0},\,y_{0},\,\theta_{0}+\omega t)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -175,7 +184,7 @@ status collapsed
|
|||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Caption
|
||||
\begin_inset Caption Standard
|
||||
|
||||
\begin_layout Plain Layout
|
||||
Robot moving along a circular trajectory.
|
||||
|
|
@ -196,20 +205,20 @@ However, if we combine translation and rotation, the story breaks down!
|
|||
We cannot write
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0}+\omega t)
|
||||
(x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0}+\omega t)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
The reason is that, if we move the robot a tiny bit according to the velocity
|
||||
vector
|
||||
\begin_inset Formula $(v_{x},\, v_{y},\,\omega)$
|
||||
\begin_inset Formula $(v_{x},\,v_{y},\,\omega)$
|
||||
\end_inset
|
||||
|
||||
, we have (to first order)
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(x_{\delta},\, y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\, y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta)
|
||||
(x_{\delta},\,y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\,y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -255,7 +264,7 @@ status open
|
|||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Caption
|
||||
\begin_inset Caption Standard
|
||||
|
||||
\begin_layout Plain Layout
|
||||
\begin_inset CommandInset label
|
||||
|
|
@ -290,7 +299,7 @@ To make progress, we have to be more precise about how the robot behaves.
|
|||
as
|
||||
\begin_inset Formula
|
||||
\[
|
||||
T_{1}T_{2}=(x_{1},\, y_{1},\,\theta_{1})(x_{2},\, y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\, y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2})
|
||||
T_{1}T_{2}=(x_{1},\,y_{1},\,\theta_{1})(x_{2},\,y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\,y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2})
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1600,13 +1609,13 @@ Hence, an alternative way of writing down elements of
|
|||
\end_inset
|
||||
|
||||
is as the ordered pair
|
||||
\begin_inset Formula $(R,\, t)$
|
||||
\begin_inset Formula $(R,\,t)$
|
||||
\end_inset
|
||||
|
||||
, with composition defined a
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1})
|
||||
(R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1})
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -2569,13 +2578,13 @@ where
|
|||
\end_inset
|
||||
|
||||
is as the ordered pair
|
||||
\begin_inset Formula $(R,\, t)$
|
||||
\begin_inset Formula $(R,\,t)$
|
||||
\end_inset
|
||||
|
||||
, with composition defined as
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1})
|
||||
(R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1})
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -2944,6 +2953,218 @@ p\\
|
|||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Section
|
||||
3D Similarity Transformations
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The group of 3D similarity transformations
|
||||
\begin_inset Formula $Sim(3)$
|
||||
\end_inset
|
||||
|
||||
is the set of
|
||||
\begin_inset Formula $4\times4$
|
||||
\end_inset
|
||||
|
||||
invertible matrices of the form
|
||||
\begin_inset Formula
|
||||
\[
|
||||
T\define\left[\begin{array}{cc}
|
||||
R & t\\
|
||||
0 & s^{-1}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
where
|
||||
\begin_inset Formula $s$
|
||||
\end_inset
|
||||
|
||||
is a scalar.
|
||||
There are several different conventions in use for the Lie algebra generators,
|
||||
but we use
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G^{1}=\left(\begin{array}{cccc}
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & -1 & 0\\
|
||||
0 & 1 & 0 & 0\\
|
||||
0 & 0 & 0 & 0
|
||||
\end{array}\right)\mbox{}G^{2}=\left(\begin{array}{cccc}
|
||||
0 & 0 & 1 & 0\\
|
||||
0 & 0 & 0 & 0\\
|
||||
-1 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0
|
||||
\end{array}\right)\mbox{ }G^{3}=\left(\begin{array}{cccc}
|
||||
0 & -1 & 0 & 0\\
|
||||
1 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0
|
||||
\end{array}\right)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G^{4}=\left(\begin{array}{cccc}
|
||||
0 & 0 & 0 & 1\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0
|
||||
\end{array}\right)\mbox{}G^{5}=\left(\begin{array}{cccc}
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 1\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0
|
||||
\end{array}\right)\mbox{ }G^{6}=\left(\begin{array}{cccc}
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 1\\
|
||||
0 & 0 & 0 & 0
|
||||
\end{array}\right)\mbox{ }G^{7}=\left(\begin{array}{cccc}
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & -1
|
||||
\end{array}\right)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsection
|
||||
Actions
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The action of
|
||||
\begin_inset Formula $\SEthree$
|
||||
\end_inset
|
||||
|
||||
on 3D points is done by embedding the points in
|
||||
\begin_inset Formula $\mathbb{R}^{4}$
|
||||
\end_inset
|
||||
|
||||
by using homogeneous coordinates
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\hat{q}=\left[\begin{array}{c}
|
||||
q\\
|
||||
s^{-1}
|
||||
\end{array}\right]=\left[\begin{array}{c}
|
||||
Rp+t\\
|
||||
s^{-1}
|
||||
\end{array}\right]=\left[\begin{array}{cc}
|
||||
R & t\\
|
||||
0 & s^{-1}
|
||||
\end{array}\right]\left[\begin{array}{c}
|
||||
p\\
|
||||
1
|
||||
\end{array}\right]=T\hat{p}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
The derivative
|
||||
\begin_inset Formula $D_{1}f(\xi)$
|
||||
\end_inset
|
||||
|
||||
in an incremental change
|
||||
\begin_inset Formula $\xi$
|
||||
\end_inset
|
||||
|
||||
to
|
||||
\begin_inset Formula $T$
|
||||
\end_inset
|
||||
|
||||
is given by
|
||||
\begin_inset Formula $TH(p)$
|
||||
\end_inset
|
||||
|
||||
where
|
||||
\begin_inset Formula
|
||||
\[
|
||||
H(p)=G_{jk}^{i}p^{j}=\left(\begin{array}{ccccccc}
|
||||
0 & z & -y & 1 & 0 & 0 & 0\\
|
||||
-z & 0 & x & 0 & 1 & 0 & 0\\
|
||||
y & -x & 0 & 0 & 0 & 1 & 0\\
|
||||
0 & 0 & 0 & 0 & 0 & 0 & -1
|
||||
\end{array}\right)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
In other words
|
||||
\begin_inset Formula
|
||||
\[
|
||||
D_{1}f(\xi)=\left[\begin{array}{cc}
|
||||
R & t\\
|
||||
0 & s^{-1}
|
||||
\end{array}\right]\left[\begin{array}{ccc}
|
||||
-\left[p\right]_{x} & I_{3} & 0\\
|
||||
0 & 0 & -1
|
||||
\end{array}\right]=\left[\begin{array}{ccc}
|
||||
-R\left[p\right]_{x} & R & -t\\
|
||||
0 & 0 & -s^{-1}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
This is the derivative for the action on homogeneous coordinates.
|
||||
Switching back to non-homogeneous coordinates is done by
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\left[\begin{array}{c}
|
||||
q\\
|
||||
a
|
||||
\end{array}\right]\rightarrow q/a
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
with derivative
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\left[\begin{array}{cc}
|
||||
a^{-1}I_{3} & -qa^{-2}\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
For
|
||||
\begin_inset Formula $a=s^{-1}$
|
||||
\end_inset
|
||||
|
||||
we obtain
|
||||
\begin_inset Formula
|
||||
\[
|
||||
D_{1}f(\xi)=\left[\begin{array}{cc}
|
||||
sI_{3} & -qs^{2}\end{array}\right]\left[\begin{array}{ccc}
|
||||
-R\left[p\right]_{x} & R & -t\\
|
||||
0 & 0 & -s^{-1}
|
||||
\end{array}\right]=\left[\begin{array}{ccc}
|
||||
-sR\left[p\right]_{x} & sR & -st+qs\end{array}\right]=\left[\begin{array}{ccc}
|
||||
-sR\left[p\right]_{x} & sR & sRp\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Newpage pagebreak
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Section
|
||||
|
|
|
|||
Binary file not shown.
45
doc/math.lyx
45
doc/math.lyx
|
|
@ -1,5 +1,5 @@
|
|||
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 413
|
||||
#LyX 2.1 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 474
|
||||
\begin_document
|
||||
\begin_header
|
||||
\textclass article
|
||||
|
|
@ -17,13 +17,13 @@ theorems-ams-bytype
|
|||
\font_roman times
|
||||
\font_sans default
|
||||
\font_typewriter default
|
||||
\font_math auto
|
||||
\font_default_family rmdefault
|
||||
\use_non_tex_fonts false
|
||||
\font_sc false
|
||||
\font_osf false
|
||||
\font_sf_scale 100
|
||||
\font_tt_scale 100
|
||||
|
||||
\graphics default
|
||||
\default_output_format default
|
||||
\output_sync 0
|
||||
|
|
@ -34,15 +34,24 @@ theorems-ams-bytype
|
|||
\use_hyperref false
|
||||
\papersize default
|
||||
\use_geometry true
|
||||
\use_amsmath 1
|
||||
\use_esint 0
|
||||
\use_mhchem 1
|
||||
\use_mathdots 1
|
||||
\use_package amsmath 1
|
||||
\use_package amssymb 1
|
||||
\use_package cancel 1
|
||||
\use_package esint 0
|
||||
\use_package mathdots 1
|
||||
\use_package mathtools 1
|
||||
\use_package mhchem 1
|
||||
\use_package stackrel 0
|
||||
\use_package stmaryrd 1
|
||||
\use_package undertilde 1
|
||||
\cite_engine basic
|
||||
\cite_engine_type default
|
||||
\biblio_style plain
|
||||
\use_bibtopic false
|
||||
\use_indices false
|
||||
\paperorientation portrait
|
||||
\suppress_date false
|
||||
\justification true
|
||||
\use_refstyle 0
|
||||
\index Index
|
||||
\shortcut idx
|
||||
|
|
@ -1603,11 +1612,7 @@ name "sec:Derivatives-of-Actions"
|
|||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The (usual) action of an
|
||||
\begin_inset Formula $n$
|
||||
\end_inset
|
||||
|
||||
-dimensional matrix group
|
||||
The (usual) action of a matrix group
|
||||
\begin_inset Formula $G$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1632,7 +1637,7 @@ Since this is a function defined on the product
|
|||
\end_inset
|
||||
|
||||
the derivative is a linear transformation
|
||||
\begin_inset Formula $Df:\Multi{2n}n$
|
||||
\begin_inset Formula $Df:\Multi{m+n}n$
|
||||
\end_inset
|
||||
|
||||
with
|
||||
|
|
@ -1643,7 +1648,15 @@ Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p)
|
|||
|
||||
\end_inset
|
||||
|
||||
where
|
||||
\begin_inset Formula $m$
|
||||
\end_inset
|
||||
|
||||
is the dimensionality of the manifold
|
||||
\begin_inset Formula $G$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Theorem
|
||||
|
|
@ -1672,7 +1685,7 @@ H(p) & I_{n}\end{array}\right]
|
|||
\end_inset
|
||||
|
||||
with
|
||||
\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$
|
||||
\begin_inset Formula $H:\Reals m\rightarrow\Reals{n\times m}$
|
||||
\end_inset
|
||||
|
||||
a linear mapping that depends on
|
||||
|
|
@ -1799,7 +1812,7 @@ with
|
|||
\end_inset
|
||||
|
||||
an
|
||||
\begin_inset Formula $n\times n$
|
||||
\begin_inset Formula $n\times m$
|
||||
\end_inset
|
||||
|
||||
matrix that depends on
|
||||
|
|
@ -3377,7 +3390,7 @@ Retractions
|
|||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\renewcommand{\retract}{\mathcal{R}}
|
||||
\newcommand{\retract}{\mathcal{R}}
|
||||
{\mathcal{R}}
|
||||
\end_inset
|
||||
|
||||
|
|
|
|||
BIN
doc/math.pdf
BIN
doc/math.pdf
Binary file not shown.
|
|
@ -67,19 +67,19 @@ Similarity3 Similarity3::operator*(const Similarity3& T) const {
|
|||
}
|
||||
|
||||
Similarity3 Similarity3::inverse() const {
|
||||
Rot3 Rt = R_.inverse();
|
||||
Point3 sRt = R_.inverse() * (-s_ * t_);
|
||||
const Rot3 Rt = R_.inverse();
|
||||
const Point3 sRt = Rt * (-s_ * t_);
|
||||
return Similarity3(Rt, sRt, 1.0 / s_);
|
||||
}
|
||||
|
||||
Point3 Similarity3::transform_from(const Point3& p, //
|
||||
OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
|
||||
Point3 q = R_ * p + t_;
|
||||
const Point3 q = R_ * p + t_;
|
||||
if (H1) {
|
||||
const Matrix3 R = R_.matrix();
|
||||
Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
// TODO(frank): explain the derivative in lambda
|
||||
*H1 << DR, s_ * R, s_ * p.vector();
|
||||
// For this derivative, see LieGroups.pdf
|
||||
const Matrix3 sR = s_ * R_.matrix();
|
||||
const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
*H1 << DR, sR, sR * p.vector();
|
||||
}
|
||||
if (H2)
|
||||
*H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
|
||||
|
|
@ -94,7 +94,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) {
|
|||
// http://www.ethaneade.org/latex2html/lie/node29.html
|
||||
const auto w = xi.head<3>();
|
||||
const auto u = xi.segment<3>(3);
|
||||
double lambda = xi[6];
|
||||
const double lambda = xi[6];
|
||||
Matrix4 W;
|
||||
W << skewSymmetric(w), u, 0, 0, 0, -lambda;
|
||||
return W;
|
||||
|
|
@ -106,81 +106,43 @@ Matrix7 Similarity3::AdjointMap() const {
|
|||
const Vector3 t = t_.vector();
|
||||
const Matrix3 A = s_ * skewSymmetric(t) * R;
|
||||
Matrix7 adj;
|
||||
adj <<
|
||||
R, Z_3x3, Matrix31::Zero(), // 3*7
|
||||
A, s_ * R, -s_ * t, // 3*7
|
||||
Matrix16::Zero(), 1; // 1*7
|
||||
adj << R, Z_3x3, Matrix31::Zero(), // 3*7
|
||||
A, s_ * R, -s_ * t, // 3*7
|
||||
Matrix16::Zero(), 1; // 1*7
|
||||
return adj;
|
||||
}
|
||||
|
||||
Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
|
||||
// http://www.ethaneade.org/latex2html/lie/node29.html
|
||||
double lambda2 = lambda * lambda;
|
||||
double theta2 = w.transpose() * w;
|
||||
double theta = sqrt(theta2);
|
||||
double A, B, C;
|
||||
// TODO(frank): eliminate copy/paste
|
||||
if (theta2 > 1e-9 && lambda2 > 1e-9) {
|
||||
const double theta2 = w.transpose() * w;
|
||||
double Y, Z, W;
|
||||
if (theta2 > 1e-9) {
|
||||
const double theta = sqrt(theta2);
|
||||
const double X = sin(theta) / theta;
|
||||
const double Y = (1 - cos(theta)) / theta2;
|
||||
const double Z = (1 - X) / theta2;
|
||||
const double W = (0.5 - Y) / theta2;
|
||||
const double alpha = lambda2 / (lambda2 + theta2);
|
||||
const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
|
||||
/ (lambda2 * lambda);
|
||||
const double upsilon = Z - (lambda * W);
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
B = alpha * (beta - gamma) + gamma;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
} else if (theta2 <= 1e-9 && lambda2 > 1e-9) {
|
||||
//Taylor series expansions
|
||||
const double Y = 0.5 - theta2 / 24.0;
|
||||
const double Z = 1.0 / 6.0 - theta2 / 120.0;
|
||||
const double W = 1.0 / 24.0 - theta2 / 720.0;
|
||||
const double alpha = lambda2 / (lambda2 + theta2);
|
||||
const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
|
||||
/ (lambda2 * lambda);
|
||||
const double upsilon = Z - (lambda * W);
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
B = alpha * (beta - gamma) + gamma;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
} else if (theta2 > 1e-9 && lambda2 <= 1e-9) {
|
||||
const double X = sin(theta) / theta;
|
||||
const double Y = (1 - cos(theta)) / theta2;
|
||||
const double Z = (1 - X) / theta2;
|
||||
const double W = (0.5 - Y) / theta2;
|
||||
const double alpha = lambda2 / (lambda2 + theta2);
|
||||
const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0
|
||||
- (lambda * lambda2) / 120;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120
|
||||
- (lambda * lambda2) / 720;
|
||||
const double upsilon = Z - (lambda * W);
|
||||
if (lambda < 1e-9) {
|
||||
A = 1 - lambda / 2.0 + lambda2 / 6.0;
|
||||
} else {
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
}
|
||||
B = alpha * (beta - gamma) + gamma;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
Y = (1 - cos(theta)) / theta2;
|
||||
Z = (1 - X) / theta2;
|
||||
W = (0.5 - Y) / theta2;
|
||||
} else {
|
||||
const double Y = 0.5 - theta2 / 24.0;
|
||||
const double Z = 1.0 / 6.0 - theta2 / 120.0;
|
||||
const double W = 1.0 / 24.0 - theta2 / 720.0;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double upsilon = Z - (lambda * W);
|
||||
if (lambda < 1e-9) {
|
||||
A = 1 - lambda / 2.0 + lambda2 / 6.0;
|
||||
} else {
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
}
|
||||
B = gamma;
|
||||
C = upsilon;
|
||||
// Taylor series expansion for theta=0, X not needed (as is 1)
|
||||
Y = 0.5 - theta2 / 24.0;
|
||||
Z = 1.0 / 6.0 - theta2 / 120.0;
|
||||
W = 1.0 / 24.0 - theta2 / 720.0;
|
||||
}
|
||||
const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda;
|
||||
double A, alpha = 0.0, beta, mu;
|
||||
if (lambda2 > 1e-9) {
|
||||
A = (1.0 - exp(-lambda)) / lambda;
|
||||
alpha = 1.0 / (1.0 + theta2 / lambda2);
|
||||
beta = (exp(-lambda) - 1 + lambda) / lambda2;
|
||||
mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3;
|
||||
} else {
|
||||
A = 1.0 - lambda / 2.0 + lambda2 / 6.0;
|
||||
beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;
|
||||
mu = 1.0 / 6.0 - lambda / 24.0 + lambda2 / 120.0 - lambda3 / 720.0;
|
||||
}
|
||||
const double gamma = Y - (lambda * Z), upsilon = Z - (lambda * W);
|
||||
const double B = alpha * (beta - gamma) + gamma;
|
||||
const double C = alpha * (mu - upsilon) + upsilon;
|
||||
const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]);
|
||||
return A * I_3x3 + B * Wx + C * Wx * Wx;
|
||||
}
|
||||
|
|
@ -218,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
|
|||
const Matrix4 Similarity3::matrix() const {
|
||||
Matrix4 T;
|
||||
T.topRows<3>() << R_.matrix(), t_.vector();
|
||||
T.bottomRows<1>() << 0, 0, 0, 1.0/s_;
|
||||
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
|
||||
return T;
|
||||
}
|
||||
|
||||
|
|
@ -226,4 +188,4 @@ Similarity3::operator Pose3() const {
|
|||
return Pose3(R_, s_ * t_);
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -45,7 +45,8 @@ static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
|
|||
static const double s = 4;
|
||||
static const Similarity3 id;
|
||||
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
|
||||
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1);
|
||||
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1),
|
||||
Point3(3.5, -8.2, 4.2), 1);
|
||||
static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
|
||||
static const Similarity3 T4(R, P, s);
|
||||
static const Similarity3 T5(R, P, 10);
|
||||
|
|
@ -187,11 +188,7 @@ TEST(Similarity3, manifold_first_order) {
|
|||
// Return as a 4*4 Matrix
|
||||
TEST(Similarity3, Matrix) {
|
||||
Matrix4 expected;
|
||||
expected <<
|
||||
1, 0, 0, 1,
|
||||
0, 1, 0, 1,
|
||||
0, 0, 1, 0,
|
||||
0, 0, 0, 0.5;
|
||||
expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5;
|
||||
Matrix4 actual = T6.matrix();
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
|
@ -200,12 +197,12 @@ TEST(Similarity3, Matrix) {
|
|||
// Exponential and log maps
|
||||
TEST(Similarity3, ExpLogMap) {
|
||||
Vector7 delta;
|
||||
delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7;
|
||||
delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
|
||||
Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta));
|
||||
EXPECT(assert_equal(delta, actual));
|
||||
|
||||
Vector7 zeros;
|
||||
zeros << 0,0,0,0,0,0,0;
|
||||
zeros << 0, 0, 0, 0, 0, 0, 0;
|
||||
Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity());
|
||||
EXPECT(assert_equal(zeros, logIdentity));
|
||||
|
||||
|
|
@ -214,7 +211,8 @@ TEST(Similarity3, ExpLogMap) {
|
|||
EXPECT(assert_equal(expZero, ident));
|
||||
|
||||
// Compare to matrix exponential, using expm in Lie.h
|
||||
EXPECT(assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
|
||||
EXPECT(
|
||||
assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -237,8 +235,8 @@ TEST(Similarity3, GroupAction) {
|
|||
EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa)));
|
||||
EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa)));
|
||||
|
||||
Similarity3 Tc(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 1.0);
|
||||
Similarity3 Td(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 2.0);
|
||||
Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0);
|
||||
Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0);
|
||||
EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa)));
|
||||
EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa)));
|
||||
|
||||
|
|
@ -249,10 +247,10 @@ TEST(Similarity3, GroupAction) {
|
|||
Point3 q(1, 2, 3);
|
||||
for (const auto T : { T1, T2, T3, T4, T5, T6 }) {
|
||||
Point3 q(1, 0, 0);
|
||||
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T1, q);
|
||||
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T1, q);
|
||||
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q);
|
||||
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q);
|
||||
Matrix actualH1, actualH2;
|
||||
T1.transform_from(q, actualH1, actualH2);
|
||||
T.transform_from(q, actualH1, actualH2);
|
||||
EXPECT(assert_equal(H1, actualH1));
|
||||
EXPECT(assert_equal(H2, actualH2));
|
||||
}
|
||||
|
|
@ -387,25 +385,25 @@ TEST(Similarity3, AlignScaledPointClouds) {
|
|||
TEST(Similarity3 , Invariants) {
|
||||
Similarity3 id;
|
||||
|
||||
EXPECT(check_group_invariants(id,id));
|
||||
EXPECT(check_group_invariants(id,T3));
|
||||
EXPECT(check_group_invariants(T2,id));
|
||||
EXPECT(check_group_invariants(T2,T3));
|
||||
EXPECT(check_group_invariants(id, id));
|
||||
EXPECT(check_group_invariants(id, T3));
|
||||
EXPECT(check_group_invariants(T2, id));
|
||||
EXPECT(check_group_invariants(T2, T3));
|
||||
|
||||
EXPECT(check_manifold_invariants(id,id));
|
||||
EXPECT(check_manifold_invariants(id,T3));
|
||||
EXPECT(check_manifold_invariants(T2,id));
|
||||
EXPECT(check_manifold_invariants(T2,T3));
|
||||
EXPECT(check_manifold_invariants(id, id));
|
||||
EXPECT(check_manifold_invariants(id, T3));
|
||||
EXPECT(check_manifold_invariants(T2, id));
|
||||
EXPECT(check_manifold_invariants(T2, T3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3 , LieGroupDerivatives) {
|
||||
Similarity3 id;
|
||||
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue