Unit test for essential matrix with prototype code, and lyx file with derivatives
Frank Dellaert
parent
b4139842a1
commit
6459053cf9
16
.cproject
16
.cproject
|
|
@ -904,14 +904,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testEssentialMatrix.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testEssentialMatrix.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>
|
||||
|
|
@ -1916,6 +1908,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testEssentialMatrix.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testEssentialMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,120 @@
|
|||
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 413
|
||||
\begin_document
|
||||
\begin_header
|
||||
\textclass article
|
||||
\use_default_options true
|
||||
\maintain_unincluded_children false
|
||||
\language english
|
||||
\language_package default
|
||||
\inputencoding auto
|
||||
\fontencoding global
|
||||
\font_roman default
|
||||
\font_sans default
|
||||
\font_typewriter default
|
||||
\font_default_family default
|
||||
\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
|
||||
\bibtex_command default
|
||||
\index_command default
|
||||
\paperfontsize 11
|
||||
\spacing single
|
||||
\use_hyperref false
|
||||
\papersize default
|
||||
\use_geometry true
|
||||
\use_amsmath 1
|
||||
\use_esint 1
|
||||
\use_mhchem 1
|
||||
\use_mathdots 1
|
||||
\cite_engine basic
|
||||
\use_bibtopic false
|
||||
\use_indices false
|
||||
\paperorientation portrait
|
||||
\suppress_date false
|
||||
\use_refstyle 1
|
||||
\index Index
|
||||
\shortcut idx
|
||||
\color #008000
|
||||
\end_index
|
||||
\leftmargin 1in
|
||||
\topmargin 1in
|
||||
\rightmargin 1in
|
||||
\bottommargin 1in
|
||||
\secnumdepth 3
|
||||
\tocdepth 3
|
||||
\paragraph_separation indent
|
||||
\paragraph_indentation default
|
||||
\quotes_language english
|
||||
\papercolumns 1
|
||||
\papersides 1
|
||||
\paperpagestyle default
|
||||
\tracking_changes false
|
||||
\output_changes false
|
||||
\html_math_output 0
|
||||
\html_css_as_file 0
|
||||
\html_be_strict false
|
||||
\end_header
|
||||
|
||||
\begin_body
|
||||
|
||||
\begin_layout Standard
|
||||
Derivative of EssentialMatrix epipolar error.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
With respect to orientation:
|
||||
\begin_inset Formula
|
||||
\[
|
||||
e(\omega)=a^{T}[t]_{\times}Re^{\omega}b=a^{T}Ee^{\omega}b
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\frac{\partial e(\omega)}{\partial v}=a^{T}E[b]_{\times}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
With respect to tangent to sphere:
|
||||
\begin_inset Formula
|
||||
\[
|
||||
e(v)=a^{T}(Bv\times Rb)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\frac{\partial e(v)}{\partial v}=a^{T}\frac{\partial(Bv\times Rb)}{\partial v}=a^{T}[-Rb]_{\times}B=a^{T}R[-b]_{\times}RB
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula
|
||||
\[
|
||||
(1*3)(3*3)(3*2)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\end_body
|
||||
\end_document
|
||||
|
|
@ -0,0 +1,328 @@
|
|||
/*
|
||||
* @file testEssentialMatrix.cpp
|
||||
* @brief Test EssentialMatrix class
|
||||
* @author Frank Dellaert
|
||||
* @date December 17, 2013
|
||||
*/
|
||||
|
||||
//#include <gtsam/geometry/EssentialMatrix.h>
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.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;
|
||||
|
||||
/**
|
||||
* An essential matrix is like a Pose3, except with translation up to scale
|
||||
* It is named after the 3*3 matrix aEb = [aTb]x aRb from computer vision,
|
||||
* but here we choose instead to parameterize it as a (Rot3,Sphere2) pair.
|
||||
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
|
||||
*/
|
||||
class EssentialMatrix: public DerivedValue<EssentialMatrix> {
|
||||
|
||||
private:
|
||||
|
||||
Rot3 aRb_; ///< Rotation between a and b
|
||||
Sphere2 aTb_; ///< translation direction from a to b
|
||||
Matrix E_; ///< Essential matrix
|
||||
|
||||
/// Static function to convert Point2 to 3D
|
||||
static Point3 Upgrade(const Point2& p) {
|
||||
return Point3(p.x(), p.y(), 1);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/// Static function to convert Point2 to homogeneous coordinates
|
||||
static Vector Homogeneous(const Point2& p) {
|
||||
return Vector(3) << p.x(), p.y(), 1;
|
||||
}
|
||||
|
||||
/// @name Constructors and named constructors
|
||||
/// @{
|
||||
|
||||
/// Construct from rotation and translation
|
||||
EssentialMatrix(const Rot3& aRb, const Sphere2& aTb) :
|
||||
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Value
|
||||
/// @{
|
||||
|
||||
/// Return the dimensionality of the tangent space
|
||||
virtual size_t dim() const {
|
||||
return 5;
|
||||
}
|
||||
|
||||
/// Retract delta to manifold
|
||||
virtual EssentialMatrix retract(const Vector& xi) const {
|
||||
assert(xi.size()==5);
|
||||
Vector3 omega(sub(xi, 0, 3));
|
||||
Vector2 z(sub(xi, 3, 5));
|
||||
Rot3 R = aRb_.retract(omega);
|
||||
Sphere2 t = aTb_.retract(z);
|
||||
return EssentialMatrix(R, t);
|
||||
}
|
||||
|
||||
/// Compute the coordinates in the tangent space
|
||||
virtual Vector localCoordinates(const EssentialMatrix& value) const {
|
||||
return Vector(5) << 0, 0, 0, 0, 0;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const string& s) const {
|
||||
cout << s;
|
||||
aRb_.print("R:\n");
|
||||
aTb_.print("d: ");
|
||||
}
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const EssentialMatrix& other, double tol) const {
|
||||
return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Essential matrix methods
|
||||
/// @{
|
||||
|
||||
/// Rotation
|
||||
const Rot3& rotation() const {
|
||||
return aRb_;
|
||||
}
|
||||
|
||||
/// Direction
|
||||
const Sphere2& direction() const {
|
||||
return aTb_;
|
||||
}
|
||||
|
||||
/// Return 3*3 matrix representation
|
||||
const Matrix& matrix() const {
|
||||
return E_;
|
||||
}
|
||||
|
||||
/// epipolar error, algebraic
|
||||
double error(const Vector& vA, const Vector& vB, //
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) {
|
||||
H->resize(1, 5);
|
||||
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
|
||||
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
|
||||
* aTb_.getBasis();
|
||||
*H << HR, HD;
|
||||
}
|
||||
return dot(vA, E_ * vB);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Factor that evaluates epipolar error p'Ep for given essential matrix
|
||||
*/
|
||||
class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
|
||||
|
||||
Point2 pA_, pB_; ///< Measurements in image A and B
|
||||
Vector vA_, vB_; ///< Homogeneous versions
|
||||
|
||||
typedef NoiseModelFactor1<EssentialMatrix> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, key), pA_(pA), pB_(pB), //
|
||||
vA_(EssentialMatrix::Homogeneous(pA)), //
|
||||
vB_(EssentialMatrix::Homogeneous(pB)) {
|
||||
}
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
Base::print(s);
|
||||
std::cout << " EssentialMatrixFactor with measurements\n ("
|
||||
<< pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
|
||||
<< ")'" << endl;
|
||||
}
|
||||
|
||||
/// vector of errors returns 1D vector
|
||||
Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
|
||||
boost::none) const {
|
||||
return (Vector(1) << E.error(vA_, vB_, H));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// Create two cameras and corresponding essential matrix E
|
||||
Rot3 aRb = Rot3::yaw(M_PI_2);
|
||||
Point3 aTb(0.1, 0, 0);
|
||||
Pose3 identity, aPb(aRb, aTb);
|
||||
typedef CalibratedCamera Cam;
|
||||
Cam cameraA(identity), cameraB(aPb);
|
||||
Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix();
|
||||
|
||||
// Create test data, we need at least 5 points
|
||||
Point3 P[5] = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
|
||||
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
|
||||
|
||||
// Project points in both cameras
|
||||
vector<Point2> pA(5), pB(5);
|
||||
vector<Point2>::iterator //
|
||||
it1 = std::transform(P, P + 5, pA.begin(),
|
||||
boost::bind(&Cam::project, &cameraA, _1, boost::none, boost::none)), //
|
||||
it2 = std::transform(P, P + 5, pB.begin(),
|
||||
boost::bind(&Cam::project, &cameraB, _1, boost::none, boost::none));
|
||||
|
||||
// Converto to homogenous coordinates
|
||||
vector<Vector> vA(5), vB(5);
|
||||
vector<Vector>::iterator //
|
||||
it3 = std::transform(pA.begin(), pA.end(), vA.begin(),
|
||||
&EssentialMatrix::Homogeneous), //
|
||||
it4 = std::transform(pB.begin(), pB.end(), vB.begin(),
|
||||
&EssentialMatrix::Homogeneous);
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, testData) {
|
||||
// Check E matrix
|
||||
Matrix expected(3, 3);
|
||||
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
|
||||
EXPECT(assert_equal(expected, aEb_matrix));
|
||||
|
||||
// Check some projections
|
||||
EXPECT(assert_equal(Point2(0,0),pA[0]));
|
||||
EXPECT(assert_equal(Point2(0,0.1),pB[0]));
|
||||
EXPECT(assert_equal(Point2(0,-1),pA[4]));
|
||||
EXPECT(assert_equal(Point2(-1,0.2),pB[4]));
|
||||
|
||||
// Check homogeneous version
|
||||
EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB[4]));
|
||||
|
||||
// Check epipolar constraint
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT_DOUBLES_EQUAL(0, vA[i].transpose() * aEb_matrix * vB[i], 1e-8);
|
||||
|
||||
// Check epipolar constraint
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA[i],vB[i]), 1e-8);
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, equality) {
|
||||
// EssentialMatrix actual, expected;
|
||||
// EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, retract1) {
|
||||
EssentialMatrix expected(aRb.retract((Vector(3) << 0.1, 0, 0)), aTb);
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, retract2) {
|
||||
EssentialMatrix expected(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, factor) {
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
|
||||
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
EssentialMatrixFactor factor(1, pA[i], pB[i], model);
|
||||
|
||||
// Check evaluation
|
||||
Vector expected(1);
|
||||
expected << 0;
|
||||
Matrix HActual;
|
||||
Vector actual = factor.evaluateError(trueE, HActual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
Matrix HExpected;
|
||||
HExpected = numericalDerivative11<EssentialMatrix>(
|
||||
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
|
||||
boost::none), trueE);
|
||||
|
||||
// Verify the Jacobian is correct
|
||||
CHECK(assert_equal(HExpected, HActual, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, fromConstraints) {
|
||||
// Here we want to optimize directly on essential matrix constraints
|
||||
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
|
||||
// but GTSAM does the equivalent anyway, provided we give the right
|
||||
// factors. In this case, the factors are the constraints.
|
||||
|
||||
// We start with a factor graph and add constraints to it
|
||||
// Noise sigma is 1cm, assuming metric measurements
|
||||
NonlinearFactorGraph graph;
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1,0.01);
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
graph.add(EssentialMatrixFactor(1, pA[i], pB[i], model));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
truth.insert(1, trueE);
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
|
||||
|
||||
// Check error at initial estimate
|
||||
Values initial;
|
||||
EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
|
||||
initial.insert(1, initialE);
|
||||
EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2);
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtParams parameters;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
// Check result
|
||||
EssentialMatrix actual = result.at<EssentialMatrix>(1);
|
||||
EXPECT(assert_equal(trueE, actual,1e-1));
|
||||
|
||||
// Check error at result
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
|
||||
// Check errors individually
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT_DOUBLES_EQUAL(0, actual.error(vA[i],vB[i]), 1e-6);
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue