diff --git a/.cproject b/.cproject index 2dbaf2d0f..19005c63f 100644 --- a/.cproject +++ b/.cproject @@ -912,6 +912,14 @@ true true + + make + -j5 + testRotateFactor.run + true + true + true + make -j2 diff --git a/doc/math.lyx b/doc/math.lyx index 98b1f0b15..d96f1f4c8 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5385,33 +5385,12 @@ We can also define a retraction from local coordinates : \begin_inset Formula \[ -q=\retract_{p}(\xi)=\frac{p+B_{p}\xi}{\left\Vert p+B_{p}\xi\right\Vert }=\frac{p+B_{p}\xi}{\alpha} +q=\retract_{p}(\xi)=\frac{p+B_{p}\xi}{\left\Vert p+B_{p}\xi\right\Vert } \] \end_inset -\end_layout - -\begin_layout Standard -Since -\begin_inset Formula $\xihat$ -\end_inset - - is in the tangent space -\begin_inset Formula $T_{p}S^{2}$ -\end_inset - - at -\begin_inset Formula $p$ -\end_inset - -, we have -\begin_inset Formula $p^{T}\xihat=0$ -\end_inset - -. - \end_layout \begin_layout Subsubsection* @@ -5434,7 +5413,7 @@ If , we have \begin_inset Formula \[ -\xi=\frac{B^{T}q}{p^{T}q} +\xi=\frac{B_{p}^{T}q}{p^{T}q} \] \end_inset @@ -5480,13 +5459,13 @@ and because \end_inset as the scaled projection -\begin_inset Formula $B^{T}q$ +\begin_inset Formula $B_{p}^{T}q$ \end_inset : \begin_inset Formula \[ -\xi=\alpha B^{T}q +\xi=\alpha B_{p}^{T}q \] \end_inset @@ -5499,7 +5478,7 @@ To recover the scale factor \begin_inset Formula $p^{T}$ \end_inset - on both sides we have + on both sides, and we get \begin_inset Formula \[ \alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi @@ -5507,7 +5486,7 @@ To recover the scale factor \end_inset -and (since +Since \begin_inset Formula $p^{T}p=1$ \end_inset @@ -5515,11 +5494,340 @@ and (since \begin_inset Formula $p^{T}B_{p}\xi=0$ \end_inset -) we have +, we then obtain \begin_inset Formula $\alpha=1/(p^{T}q)$ \end_inset +, which completes the proof. +\end_layout + +\begin_layout Subsection +Rotation acting on a 3D Direction +\end_layout + +\begin_layout Standard +Rotating a point +\begin_inset Formula $p\in S^{2}$ +\end_inset + + on the sphere obviously yields another point +\begin_inset Formula $q=Rp\in S^{2}$ +\end_inset + +, as rotation preserves the norm. + The derivative of +\begin_inset Formula $f(R,p)$ +\end_inset + + with respect to +\begin_inset Formula $R$ +\end_inset + + can be found by equating +\begin_inset Formula +\[ +Rp+B_{Rp}\xi=R(I+\Skew{\omega})p=Rp+R\Skew{\omega}p +\] + +\end_inset + + +\begin_inset Formula +\[ +B_{Rp}\xi=-R\Skew p\omega +\] + +\end_inset + + +\begin_inset Formula +\[ +\xi=-B_{Rp}^{T}R\Skew p\omega +\] + +\end_inset + +whereas with respect to +\begin_inset Formula $p$ +\end_inset + + we have +\begin_inset Formula +\[ +Rp+B_{Rp}\xi_{q}=R(p+B_{p}\xi_{p}) +\] + +\end_inset + + +\begin_inset Formula +\[ +\xi_{q}=B_{Rp}^{T}RB_{p}\xi_{p} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +In other words, the Jacobian matrix is given by +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula +\[ +f'(R,p)=\left[\begin{array}{cc} +-B_{Rp}^{T}R\Skew p & B_{Rp}^{T}RB_{p}\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Error between 3D Directions +\end_layout + +\begin_layout Standard +We would like to define a distance metric +\begin_inset Formula $e(p,q)$ +\end_inset + + between two directions +\begin_inset Formula $p,q\in S^{2}$ +\end_inset + . + An obvious choice is +\begin_inset Formula +\[ +\theta=\cos^{-1}\left(p^{T}q\right) +\] + +\end_inset + +which is exactly the distance along the shortest path (geodesic) on the + sphere, i.e., this is the distance metric associated with the exponential. + The advantage is that it is defined everywhere, but it involves +\begin_inset Formula $\cos^{-1}$ +\end_inset + +. + The derivative with respect to a change in +\begin_inset Formula $q$ +\end_inset + +, via +\begin_inset Formula $\xi$ +\end_inset + +, is then +\begin_inset Formula +\[ +\frac{\partial\theta(p,q)}{\partial\xi}=\frac{\partial\cos^{-1}\left(p^{T}q\right)}{\partial\xi}=\frac{p^{T}B_{q}}{\sqrt{1-\left(p^{T}q\right)^{2}}} +\] + +\end_inset + +which is also undefined for +\begin_inset Formula $p=q$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +A simpler metric is derived from the retraction but only holds when +\begin_inset Formula $q\approx p$ +\end_inset + +. + It simply projects +\begin_inset Formula $q$ +\end_inset + + onto the local coordinate basis +\begin_inset Formula $B_{p}$ +\end_inset + + defined by +\begin_inset Formula $p$ +\end_inset + +, and takes the norm: +\begin_inset Formula +\[ +\theta(p,q)=\left\Vert B_{p}^{T}q\right\Vert +\] + +\end_inset + +The derivative with respect to a change in +\begin_inset Formula $q$ +\end_inset + +, via +\begin_inset Formula $\xi$ +\end_inset + +, is then +\begin_inset Formula +\[ +\frac{\partial\theta(p,q)}{\partial\xi_{q}}=\frac{\partial}{\partial\xi_{q}}\sqrt{\left(B_{p}^{T}q\right)^{2}}=\frac{1}{\sqrt{\left(B_{p}^{T}q\right)^{2}}}\left(B_{p}^{T}q\right)B_{p}^{T}B_{q}=\frac{B_{p}^{T}q}{\theta(q;p)}B_{p}^{T}B_{q} +\] + +\end_inset + +Note that this again is undefined for +\begin_inset Formula $\theta=0$ +\end_inset + +. +\end_layout + +\begin_layout Standard +For use in a probabilistic factor, a signed, vector-valued error will not + have the discontinuity: +\begin_inset Formula +\[ +\theta(p,q)=B_{p}^{T}q +\] + +\end_inset + +Note this is the inverse retraction up to a scale. + The derivative with respect to a change in +\begin_inset Formula $q$ +\end_inset + +, via +\begin_inset Formula $\xi$ +\end_inset + +, is found by +\begin_inset Formula +\[ +\frac{\partial\theta(p,q)}{\partial\xi_{q}}=B_{p}^{T}\frac{\partial q}{\partial\xi_{q}}=B_{p}^{T}B_{q} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Application +\end_layout + +\begin_layout Standard +We can use the above to find the unknown rotation between a camera and an + IMU. + If we measure the rotation between two frames as +\begin_inset Formula $c_{1}Zc_{2}$ +\end_inset + +, and the predicted rotation from the IMU is +\begin_inset Formula $i_{1}Ri_{2}$ +\end_inset + +, then we can predict +\begin_inset Formula +\[ +c_{1}Zc_{2}=iRc^{T}\cdot i_{1}Ri_{2}\cdot iRc +\] + +\end_inset + +and the axis of the incremental rotations will relate as +\begin_inset Formula +\[ +p=iRc\cdot z +\] + +\end_inset + +with +\begin_inset Formula $p$ +\end_inset + + the angular velocity axis in the IMU frame, and +\begin_inset Formula $z$ +\end_inset + + the measured axis of rotation between the two cameras. + Note this only makes sense if the rotation is non-zero. + So, given an initial estimate +\begin_inset Formula $R$ +\end_inset + + for the unknown rotation +\begin_inset Formula $iRc$ +\end_inset + + between IMU and camera, the derivative of the error is (using +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Rot3action" + +\end_inset + +) +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula +\[ +\frac{\partial\theta(Rz;p)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}B_{Rz}\frac{\partial\left(Rz\right)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}R\Skew z +\] + +\end_inset + +Here the +\begin_inset Formula $2\times3$ +\end_inset + + matrix +\begin_inset Formula $B_{Rz}^{T}\Skew z$ +\end_inset + + translates changes in +\begin_inset Formula $R$ +\end_inset + + to changes in +\begin_inset Formula $Rz$ +\end_inset + +, and the +\begin_inset Formula $1\times2$ +\end_inset + + matrix +\begin_inset Formula $B_{p}^{T}\left(Rz\right)$ +\end_inset + + describes the downstream effect on the error metric. \end_layout \begin_layout Section diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h new file mode 100644 index 000000000..428f60680 --- /dev/null +++ b/gtsam/slam/RotateFactor.h @@ -0,0 +1,68 @@ +/* + * @file RotateFactor.cpp + * @brief RotateFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor that evaluates distance between two rotated directions + */ +class RotateFactor: public NoiseModelFactor1 { + + Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z + + typedef NoiseModelFactor1 Base; + +public: + + /// Constructor + RotateFactor(Key key, const Sphere2& p, const Sphere2& z, + const SharedNoiseModel& model) : + Base(model, key), p_(p), z_(z) { + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + p_.print("p"); + z_.print("z"); + } + + /// vector of errors returns 2D vector + Vector evaluateError(const Rot3& R, + boost::optional H = boost::none) const { + Sphere2 q = R * z_; + Vector e = p_.error(q, H); + if (H) { + Matrix DR; + Sphere2::Rotate(R, z_, DR); + *H = (*H) * DR; + } + return e; + } + + /// Obsolete: vector of errors returns 1D vector + Vector evaluateError1(const Rot3& R, + boost::optional H = boost::none) const { + Sphere2 q = R * z_; + double e = p_.distance(q, H); + if (H) { + Matrix DR; + Sphere2::Rotate(R, z_, DR); + *H = (*H) * DR; + } + return (Vector(1) << e); + } + +}; +} // gtsam + diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp new file mode 100644 index 000000000..c5992d3de --- /dev/null +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -0,0 +1,101 @@ +/* + * @file testRotateFactor.cpp + * @brief Test RotateFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +//************************************************************************* +// Create some test data +// Let's assume IMU is aligned with aero (X-forward,Z down) +// And camera is looking forward. +Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); +Rot3 iRc(cameraX, cameraY, cameraZ); +// Now, let's create some rotations around IMU frame +Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); +// The corresponding rotations in the camera frame +Sphere2 z1 = iRc.inverse() * p1, z2 = iRc.inverse() * p2, // +z3 = iRc.inverse() * p3; + +// noise model +noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, 0.01); + +//************************************************************************* +TEST (RotateFactor, test) { + RotateFactor f(1, p1, z1, model); + Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + EXPECT(assert_equal((Vector(2)<<0,0), f.evaluateError(iRc), 1e-8)); + EXPECT( + assert_equal((Vector(2)<<-0.08867, -0.20197), f.evaluateError(R), 1e-5)); + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); + f.evaluateError(iRc, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative11( + boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); + f.evaluateError(R, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + +//************************************************************************* +TEST (RotateFactor, minimization) { + // Let's try to recover the correct iRc by minimizing + NonlinearFactorGraph graph; + graph.add(RotateFactor(1, p1, z1, model)); + graph.add(RotateFactor(1, p2, z2, model)); + graph.add(RotateFactor(1, p3, z3, model)); + + // Check error at ground truth + Values truth; + truth.insert(1, iRc); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + double degree = M_PI/180; + Rot3 initialE = iRc.retract(degree*(Vector(3) << 20, -20, 20)); + initial.insert(1, initialE); + EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1); + + // Optimize + LevenbergMarquardtParams parameters; + //parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + Rot3 actual = result.at(1); + EXPECT(assert_equal(iRc, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +