RotateFactor can help calibrate a rig *without* SLAM. Relevant math added to math.lyx
Frank Dellaert
parent
e0c0cb751e
commit
51f2ba2c7e
|
|
@ -912,6 +912,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRotateFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testRotateFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
364
doc/math.lyx
364
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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,68 @@
|
|||
/*
|
||||
* @file RotateFactor.cpp
|
||||
* @brief RotateFactor class
|
||||
* @author Frank Dellaert
|
||||
* @date December 17, 2013
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Factor that evaluates distance between two rotated directions
|
||||
*/
|
||||
class RotateFactor: public NoiseModelFactor1<Rot3> {
|
||||
|
||||
Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> 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<Matrix&> 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<Matrix&> 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
|
||||
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
/*
|
||||
* @file testRotateFactor.cpp
|
||||
* @brief Test RotateFactor class
|
||||
* @author Frank Dellaert
|
||||
* @date December 17, 2013
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/RotateFactor.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <vector>
|
||||
|
||||
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<Rot3>(
|
||||
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
|
||||
f.evaluateError(iRc, actual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative11<Rot3>(
|
||||
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<Rot3>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue