Merged develop into feature/Feature/FixedValues

doc/LieGroups.lyx

@@ -1,5 +1,5 @@
-#LyX 2.0 created this file. For more info see http://www.lyx.org/
+#LyX 2.1 created this file. For more info see http://www.lyx.org/
-\lyxformat 413
+\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_package amsmath 1
-\use_esint 0
+\use_package amssymb 1
-\use_mhchem 1
+\use_package cancel 1
-\use_mathdots 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
@@ -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.
@@ -255,7 +264,7 @@ status open
 \end_inset
 
 
-\begin_inset Caption
+\begin_inset Caption Standard
 
 \begin_layout Plain Layout
 \begin_inset CommandInset label
@@ -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

doc/math.lyx

@@ -1,5 +1,5 @@
-#LyX 2.0 created this file. For more info see http://www.lyx.org/
+#LyX 2.1 created this file. For more info see http://www.lyx.org/
-\lyxformat 413
+\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_package amsmath 1
-\use_esint 0
+\use_package amssymb 1
-\use_mhchem 1
+\use_package cancel 1
-\use_mathdots 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 
+The (usual) action of a matrix group 
-\begin_inset Formula $n$
-\end_inset
-
--dimensional 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
 

gtsam_unstable/geometry/Similarity3.cpp

@@ -67,19 +67,19 @@ Similarity3 Similarity3::operator*(const Similarity3& T) const {
 }
 
 Similarity3 Similarity3::inverse() const {
-  Rot3 Rt = R_.inverse();
+  const Rot3 Rt = R_.inverse();
-  Point3 sRt = R_.inverse() * (-s_ * t_);
+  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();
+    // For this derivative, see LieGroups.pdf
-    Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z());
+    const Matrix3 sR = s_ * R_.matrix();
-    // TODO(frank): explain the derivative in lambda
+    const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z());
-    *H1 << DR, s_ * R, s_ * p.vector();
+    *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,8 +106,7 @@ Matrix7 Similarity3::AdjointMap() const {
   const Vector3 t = t_.vector();
   const Matrix3 A = s_ * skewSymmetric(t) * R;
   Matrix7 adj;
-  adj <<
+  adj << R, Z_3x3, Matrix31::Zero(), // 3*7
-      R, Z_3x3, Matrix31::Zero(), // 3*7
   A, s_ * R, -s_ * t, // 3*7
   Matrix16::Zero(), 1; // 1*7
   return adj;
@@ -115,72 +114,35 @@ Matrix7 Similarity3::AdjointMap() const {
 
 Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
   // http://www.ethaneade.org/latex2html/lie/node29.html
-  double lambda2 = lambda * lambda;
+  const double theta2 = w.transpose() * w;
-  double theta2 = w.transpose() * w;
+  double Y, Z, W;
-  double theta = sqrt(theta2);
+  if (theta2 > 1e-9) {
-  double A, B, C;
+    const double theta = sqrt(theta2);
-  // TODO(frank): eliminate copy/paste
-  if (theta2 > 1e-9 && lambda2 > 1e-9) {
     const double X = sin(theta) / theta;
-    const double Y = (1 - cos(theta)) / theta2;
+    Y = (1 - cos(theta)) / theta2;
-    const double Z = (1 - X) / theta2;
+    Z = (1 - X) / theta2;
-    const double W = (0.5 - Y) / theta2;
+    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;
+    // 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;
   }
-    B = alpha * (beta - gamma) + gamma;
+  const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda;
-    C = alpha * (mu - upsilon) + upsilon;
+  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 {
-    const double Y = 0.5 - theta2 / 24.0;
+    A = 1.0 - lambda / 2.0 + lambda2 / 6.0;
-    const double Z = 1.0 / 6.0 - theta2 / 120.0;
+    beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;
-    const double W = 1.0 / 24.0 - theta2 / 720.0;
+    mu = 1.0 / 6.0 - lambda / 24.0 + lambda2 / 120.0 - lambda3 / 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;
   }
+  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;
 }
@@ -226,4 +188,4 @@ Similarity3::operator Pose3() const {
   return Pose3(R_, s_ * t_);
 }
 
-}
+} // namespace gtsam

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -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 <<
+  expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5;
-      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));
 }
@@ -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));
 }
 
 //******************************************************************************
@@ -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 H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q);
-    Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T1, 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));
   }