198 lines
6.1 KiB
C++
198 lines
6.1 KiB
C++
/*
|
|
* testHomography2.cpp
|
|
* @brief Test and estimate 2D homographies
|
|
* Created on: Feb 13, 2010
|
|
* @author: Frank Dellaert
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/assign/std/list.hpp> // for operator +=
|
|
using namespace boost::assign;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Testable.h"
|
|
#include "tensors.h"
|
|
#include "tensorInterface.h"
|
|
#include "projectiveGeometry.h"
|
|
#include "Pose3.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
using namespace tensors;
|
|
|
|
/* ************************************************************************* */
|
|
// Indices
|
|
|
|
Index<3, 'a'> a, _a;
|
|
Index<3, 'b'> b, _b;
|
|
Index<3, 'c'> c, _c;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Homography2, RealImages)
|
|
{
|
|
// 4 point correspondences MATLAB from the floor of bt001.png and bt002.png
|
|
Correspondence p1(point2h(216.841, 443.220, 1), point2h(213.528, 414.671, 1));
|
|
Correspondence p2(point2h(252.119, 363.481, 1), point2h(244.614, 348.842, 1));
|
|
Correspondence p3(point2h(316.614, 414.768, 1), point2h(303.128, 390.000, 1));
|
|
Correspondence p4(point2h(324.165, 465.463, 1), point2h(308.614, 431.129, 1));
|
|
|
|
// Homography obtained using MATLAB code
|
|
double h[3][3] = { { 0.9075, 0.0031, -0 }, { -0.1165, 0.8288, -0.0004 }, {
|
|
30.8472, 16.0449, 1 } };
|
|
Homography2 H(h);
|
|
|
|
// CHECK whether they are equivalent
|
|
CHECK(assert_equivalent(p1.second(b),H(b,a)*p1.first(a),0.05))
|
|
CHECK(assert_equivalent(p2.second(b),H(b,a)*p2.first(a),0.05))
|
|
CHECK(assert_equivalent(p3.second(b),H(b,a)*p3.first(a),0.05))
|
|
CHECK(assert_equivalent(p4.second(b),H(b,a)*p4.first(a),0.05))
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Homography test case
|
|
// 4 trivial correspondences of a translating square
|
|
Correspondence p1(point2h(0, 0, 1), point2h(4, 5, 1));
|
|
Correspondence p2(point2h(1, 0, 1), point2h(5, 5, 1));
|
|
Correspondence p3(point2h(1, 1, 1), point2h(5, 6, 1));
|
|
Correspondence p4(point2h(0, 1, 1), point2h(4, 6, 1));
|
|
|
|
double h[3][3] = { { 1, 0, 4 }, { 0, 1, 5 }, { 0, 0, 1 } };
|
|
Homography2 H(h);
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Homography2, TestCase)
|
|
{
|
|
// Check homography
|
|
list<Correspondence> correspondences;
|
|
correspondences += p1, p2, p3, p4;
|
|
BOOST_FOREACH(const Correspondence& p, correspondences)
|
|
CHECK(assert_equality(p.second(b),H(_a,b) * p.first(a)))
|
|
|
|
// Check a line
|
|
Line2h l1 = line2h(1, 0, -1); // in a
|
|
Line2h l2 = line2h(1, 0, -5); // x==5 in b
|
|
CHECK(assert_equality(l1(a), H(a,b)*l2(b)))
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Homography2, Estimate)
|
|
{
|
|
list<Correspondence> correspondences;
|
|
correspondences += p1, p2, p3, p4;
|
|
Homography2 estimatedH = estimateHomography2(correspondences);
|
|
CHECK(assert_equivalent(H(_a,b),estimatedH(_a,b)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Homography2, EstimateReverse)
|
|
{
|
|
double h[3][3] = { { 1, 0, -4 }, { 0, 1, -5 }, { 0, 0, 1 } };
|
|
Homography2 reverse(h);
|
|
|
|
list<Correspondence> correspondences;
|
|
correspondences += p1.swap(), p2.swap(), p3.swap(), p4.swap();
|
|
Homography2 estimatedH = estimateHomography2(correspondences);
|
|
CHECK(assert_equality(reverse(_a,b),estimatedH(_a,b)*(1.0/estimatedH(2,2))));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/**
|
|
* Computes the homography H(I,_T) from template to image
|
|
* given the pose tEc of the camera in the template coordinate frame.
|
|
* Assumption is Z is normal to the template, template texture in X-Y plane.
|
|
*/
|
|
Homography2 patchH(const Pose3& tEc) {
|
|
Pose3 cEt = inverse(tEc);
|
|
Rot3 cRt = cEt.rotation();
|
|
Point3 r1 = cRt.column(1), r2 = cRt.column(2), t = cEt.translation();
|
|
|
|
// TODO cleanup !!!!
|
|
// column 1
|
|
double H11 = r1.x();
|
|
double H21 = r1.y();
|
|
double H31 = r1.z();
|
|
// column 2
|
|
double H12 = r2.x();
|
|
double H22 = r2.y();
|
|
double H32 = r2.z();
|
|
// column 3
|
|
double H13 = t.x();
|
|
double H23 = t.y();
|
|
double H33 = t.z();
|
|
double data2[3][3] = { { H11, H21, H31 }, { H12, H22, H32 },
|
|
{ H13, H23, H33 } };
|
|
return Homography2(data2);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
namespace gtsam {
|
|
size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
|
|
Vector toVector(const tensors::Tensor2<3, 3>& H) {
|
|
Index<3, 'T'> _T; // covariant 2D template
|
|
Index<3, 'C'> I; // contravariant 2D camera
|
|
return toVector(H(I,_T));
|
|
}
|
|
Vector logmap(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
|
|
return toVector(A)-toVector(B); // TODO correct order ?
|
|
}
|
|
}
|
|
|
|
#include "numericalDerivative.h"
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Homography2, patchH)
|
|
{
|
|
Index<3, 'T'> _T; // covariant 2D template
|
|
Index<3, 'C'> I; // contravariant 2D camera
|
|
|
|
// data[_T][I]
|
|
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
|
|
Homography2 expected(data1);
|
|
|
|
// camera rotation, looking in negative Z
|
|
Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1));
|
|
Point3 gTc(0,0,10); // Camera location, out on the Z axis
|
|
Pose3 gEc(gRc,gTc); // Camera pose
|
|
|
|
Homography2 actual = patchH(gEc);
|
|
|
|
// GTSAM_PRINT(expected(I,_T));
|
|
// GTSAM_PRINT(actual(I,_T));
|
|
CHECK(assert_equality(expected(I,_T),actual(I,_T)));
|
|
|
|
Matrix D = numericalDerivative11<Homography2,Pose3>(patchH, gEc);
|
|
// print(D,"D");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Homography2, patchH2)
|
|
{
|
|
Index<3, 'T'> _T; // covariant 2D template
|
|
Index<3, 'C'> I; // contravariant 2D camera
|
|
|
|
// data[_T][I]
|
|
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
|
|
Homography2 expected(data1);
|
|
|
|
// camera rotation, looking in negative Z
|
|
Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1));
|
|
Point3 gTc(0,0,10); // Camera location, out on the Z axis
|
|
Pose3 gEc(gRc,gTc); // Camera pose
|
|
|
|
Homography2 actual = patchH(gEc);
|
|
|
|
// GTSAM_PRINT(expected(I,_T));
|
|
// GTSAM_PRINT(actual(I,_T));
|
|
CHECK(assert_equality(expected(I,_T),actual(I,_T)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|
|
|