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>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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">
|
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<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
|
\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_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
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Subsubsection*
|
\begin_layout Subsubsection*
|
||||||
|
|
@ -5434,7 +5413,7 @@ If
|
||||||
, we have
|
, we have
|
||||||
\begin_inset Formula
|
\begin_inset Formula
|
||||||
\[
|
\[
|
||||||
\xi=\frac{B^{T}q}{p^{T}q}
|
\xi=\frac{B_{p}^{T}q}{p^{T}q}
|
||||||
\]
|
\]
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
@ -5480,13 +5459,13 @@ and because
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
as the scaled projection
|
as the scaled projection
|
||||||
\begin_inset Formula $B^{T}q$
|
\begin_inset Formula $B_{p}^{T}q$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
:
|
:
|
||||||
\begin_inset Formula
|
\begin_inset Formula
|
||||||
\[
|
\[
|
||||||
\xi=\alpha B^{T}q
|
\xi=\alpha B_{p}^{T}q
|
||||||
\]
|
\]
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
@ -5499,7 +5478,7 @@ To recover the scale factor
|
||||||
\begin_inset Formula $p^{T}$
|
\begin_inset Formula $p^{T}$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
on both sides we have
|
on both sides, and we get
|
||||||
\begin_inset Formula
|
\begin_inset Formula
|
||||||
\[
|
\[
|
||||||
\alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi
|
\alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi
|
||||||
|
|
@ -5507,7 +5486,7 @@ To recover the scale factor
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
and (since
|
Since
|
||||||
\begin_inset Formula $p^{T}p=1$
|
\begin_inset Formula $p^{T}p=1$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -5515,11 +5494,340 @@ and (since
|
||||||
\begin_inset Formula $p^{T}B_{p}\xi=0$
|
\begin_inset Formula $p^{T}B_{p}\xi=0$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
) we have
|
, we then obtain
|
||||||
\begin_inset Formula $\alpha=1/(p^{T}q)$
|
\begin_inset Formula $\alpha=1/(p^{T}q)$
|
||||||
\end_inset
|
\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
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Section
|
\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