From c76e9aa7eb3fecc55b441d3a47ba03c066d835a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 13 Sep 2010 03:25:27 +0000 Subject: [PATCH] added testFundamental --- .cproject | 2251 ++++++++++++++-------------- .project | 2 +- geometry/Makefile.am | 2 +- geometry/tests/testFundamental.cpp | 112 ++ 4 files changed, 1265 insertions(+), 1102 deletions(-) create mode 100644 geometry/tests/testFundamental.cpp diff --git a/.cproject b/.cproject index d480c7da0..8023b0e49 100644 --- a/.cproject +++ b/.cproject @@ -5,47 +5,46 @@ - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -299,1066 +298,1118 @@ - - - make - - all - true - true - true - - - make - - clean - true - true - true - - - make - - check - true - true - true - - - make - - testGaussianConditional.run - true - true - true - - - make - - testGaussianFactor.run - true - true - true - - - make - - timeGaussianFactor.run - true - true - true - - - make - - timeVectorConfig.run - true - true - true - - - make - - testVectorBTree.run - true - true - true - - - make - - testVectorMap.run - true - true - true - - - make - - testNoiseModel.run - true - true - true - - - make - - testBayesNetPreconditioner.run - true - true - true - - - make - - testErrors.run - true - false - true - - - make - check - true - true - true - - - make - tests/testSPQRUtil.run - true - true - true - - - make - clean - true - true - true - - - make - - check - true - true - true - - - make - - tests/testGaussianJunctionTree.run - true - true - true - - - make - check - true - true - true - - - make - clean - true - true - true - - - make - testBTree.run - true - true - true - - - make - testDSF.run - true - true - true - - - make - testDSFVector.run - true - true - true - - - make - testMatrix.run - true - true - true - - - make - testSPQRUtil.run - true - true - true - - - make - testVector.run - true - true - true - - - make - timeMatrix.run - true - true - true - - - make - all - true - true - true - - - make - - all - true - true - true - - - make - - clean - true - true - true - - - make - -k - check - true - false - true - - - make - - testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - - make - - testFactorGraph.run - true - true - true - - - make - - testISAM.run - true - true - true - - - make - - testJunctionTree.run - true - true - true - - - make - - testKey.run - true - true - true - - - make - - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - - timeSymbolMaps.run - true - true - true - - - make - - check - true - true - true - - - make - - testClusterTree.run - true - true - true - - - make - - testJunctionTree.run - true - true - true - - - make - - testEliminationTree.run - true - true - true - - - make - check - true - true - true - - - make - testGaussianFactorGraph.run - true - true - true - - - make - testGaussianISAM.run - true - true - true - - - make - testGaussianISAM2.run - true - true - true - - - make - testGraph.run - true - false - true - - - make - testIterative.run - true - true - true - - - make - testNonlinearEquality.run - true - true - true - - - make - testNonlinearFactor.run - true - true - true - - - make - testNonlinearFactorGraph.run - true - true - true - - - make - testNonlinearOptimizer.run - true - true - true - - - make - testSQP.run - true - true - true - - - make - testSubgraphPreconditioner.run - true - true - true - - - make - testTupleConfig.run - true - true - true - - - make - timeGaussianFactorGraph.run - true - true - true - - - make - testBayesNetPreconditioner.run - true - true - true - - - make - testConstraintOptimizer.run - true - true - true - - - make - testInference.run - true - false - true - - - make - testGaussianBayesNet.run - true - false - true - - - make - testGaussianFactor.run - true - false - true - - - make - testJunctionTree.run - true - false - true - - - make - testSymbolicBayesNet.run - true - false - true - - - make - testSymbolicFactorGraph.run - true - false - true - - - make - - clean - true - true - true - - - make - - all - true - true - true - - - make - - testNonlinearConstraint.run - true - true - true - - - make - - testLieConfig.run - true - true - true - - - make - - testConstraintOptimizer.run - true - true - true - - - make - - install - true - true - true - - - make - - clean - true - true - true - - - make - all - true - true - true - - - make - clean - true - true - true - - - make - all - true - true - true - - - make - clean - true - true - true - - - make - all - true - true - true - - - make - clean - true - true - true - - - make - - all - true - true - true - - - make - - clean - true - true - true - - - make - clean all - true - true - true - - - make - all - true - true - true - - - make - check - true - true - true - - - make - clean - true - true - true - - - make - testPlanarSLAM.run - true - true - true - - - make - testPose2Config.run - true - true - true - - - make - testPose2Factor.run - true - true - true - - - make - testPose2Prior.run - true - true - true - - - make - testPose2SLAM.run - true - true - true - - - make - testPose3Config.run - true - true - true - - - make - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - testVSLAMConfig.run - true - true - true - - - make - testVSLAMFactor.run - true - true - true - - - make - testVSLAMGraph.run - true - true - true - - - make - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - check - true - true - true - - - make - - timeCalibratedCamera.run - true - true - true - - - make - - timeRot3.run - true - true - true - - - make - - clean - true - true - true - - - make - SimpleRotation.run - true - true - true - - - make - SLAMSelfContained.run - true - true - true - - - make - PlanarSLAMExample.run - true - true - true - - - make - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - check - true - true - true - - - make - - clean - true - true - true - - - make - check - true - true - true - - - make - - tests/testLieConfig.run - true - true - true - - - make - testRot3.run - true - true - true - - - make - testRot2.run - true - true - true - - - make - testPose3.run - true - true - true - - - make - timeRot3.run - true - true - true - - - make - testPose2.run - true - true - true - - - make - testCal3_S2.run - true - true - true - - - make - testSimpleCamera.run - true - true - true - - - make - testHomography2.run - true - true - true - - - make - testCalibratedCamera.run - true - true - true - - - make - check - true - true - true - - - make - clean - true - true - true - - - make - testPoint2.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - all - true - true - true - - - make - dist - true - true - true - - - make - - check - true - true - true - - - make - - testGaussianJunctionTree.run - true - true - true - - - make - - testGaussianFactorGraph.run - true - true - true - - - make - - timeGaussianFactorGraph.run - true - true - true - - - make - testTupleConfig.run - true - true - true - - - make - testFusionTupleConfig.run - true - true - true - - - make - - testSerialization.run - true - true - true - - - make - - check - true - true - true - - - make - - clean - true - true - true - - - make - - install - true - true - true - - - make - - all - true - true - true - - - make - SimpleRotation.run - true - true - true - - - - + + +make +all +true +true +true + + +make +clean +true +true +true + + +make +check +true +true +true + + +make +testGaussianConditional.run +true +true +true + + +make +testGaussianFactor.run +true +true +true + + +make +timeGaussianFactor.run +true +true +true + + +make +timeVectorConfig.run +true +true +true + + +make +testVectorBTree.run +true +true +true + + +make +testVectorMap.run +true +true +true + + +make +testNoiseModel.run +true +true +true + + +make +testBayesNetPreconditioner.run +true +true +true + + +make +testErrors.run +true +false +true + + +make + +check +true +true +true + + +make + +tests/testSPQRUtil.run +true +true +true + + +make + +clean +true +true +true + + +make +check +true +true +true + + +make +tests/testGaussianJunctionTree.run +true +true +true + + +make + +check +true +true +true + + +make + +clean +true +true +true + + +make + +testBTree.run +true +true +true + + +make + +testDSF.run +true +true +true + + +make + +testDSFVector.run +true +true +true + + +make + +testMatrix.run +true +true +true + + +make + +testSPQRUtil.run +true +true +true + + +make + +testVector.run +true +true +true + + +make + +timeMatrix.run +true +true +true + + +make + +all +true +true +true + + +make +all +true +true +true + + +make +clean +true +true +true + + +make +-k +check +true +false +true + + +make +testBayesTree.run +true +false +true + + +make +testBinaryBayesNet.run +true +false +true + + +make +testFactorGraph.run +true +true +true + + +make +testISAM.run +true +true +true + + +make +testJunctionTree.run +true +true +true + + +make +testKey.run +true +true +true + + +make +testOrdering.run +true +true +true + + +make +testSymbolicBayesNet.run +true +false +true + + +make +testSymbolicFactor.run +true +false +true + + +make +testSymbolicFactorGraph.run +true +false +true + + +make +timeSymbolMaps.run +true +true +true + + +make +check +true +true +true + + +make +testClusterTree.run +true +true +true + + +make +testJunctionTree.run +true +true +true + + +make +testEliminationTree.run +true +true +true + + +make + +check +true +true +true + + +make + +testGaussianFactorGraph.run +true +true +true + + +make + +testGaussianISAM.run +true +true +true + + +make + +testGaussianISAM2.run +true +true +true + + +make + +testGraph.run +true +false +true + + +make + +testIterative.run +true +true +true + + +make + +testNonlinearEquality.run +true +true +true + + +make + +testNonlinearFactor.run +true +true +true + + +make + +testNonlinearFactorGraph.run +true +true +true + + +make + +testNonlinearOptimizer.run +true +true +true + + +make + +testSQP.run +true +true +true + + +make + +testSubgraphPreconditioner.run +true +true +true + + +make + +testTupleConfig.run +true +true +true + + +make + +timeGaussianFactorGraph.run +true +true +true + + +make + +testBayesNetPreconditioner.run +true +true +true + + +make + +testConstraintOptimizer.run +true +true +true + + +make + +testInference.run +true +false +true + + +make + +testGaussianBayesNet.run +true +false +true + + +make + +testGaussianFactor.run +true +false +true + + +make + +testJunctionTree.run +true +false +true + + +make + +testSymbolicBayesNet.run +true +false +true + + +make + +testSymbolicFactorGraph.run +true +false +true + + +make +clean +true +true +true + + +make +all +true +true +true + + +make +testNonlinearConstraint.run +true +true +true + + +make +testLieConfig.run +true +true +true + + +make +testConstraintOptimizer.run +true +true +true + + +make +install +true +true +true + + +make +clean +true +true +true + + +make + +all +true +true +true + + +make + +clean +true +true +true + + +make + +all +true +true +true + + +make + +clean +true +true +true + + +make + +all +true +true +true + + +make + +clean +true +true +true + + +make +all +true +true +true + + +make +clean +true +true +true + + +make + +clean all +true +true +true + + +make + +all +true +true +true + + +make + +check +true +true +true + + +make + +clean +true +true +true + + +make + +testPlanarSLAM.run +true +true +true + + +make + +testPose2Config.run +true +true +true + + +make + +testPose2Factor.run +true +true +true + + +make + +testPose2Prior.run +true +true +true + + +make + +testPose2SLAM.run +true +true +true + + +make + +testPose3Config.run +true +true +true + + +make + +testPose3SLAM.run +true +true +true + + +make + +testSimulated2DOriented.run +true +false +true + + +make + +testVSLAMConfig.run +true +true +true + + +make + +testVSLAMFactor.run +true +true +true + + +make + +testVSLAMGraph.run +true +true +true + + +make + +testPose3Factor.run +true +true +true + + +make + +testSimulated2D.run +true +false +true + + +make + +testSimulated3D.run +true +false +true + + +make + +check +true +true +true + + +make +timeCalibratedCamera.run +true +true +true + + +make +timeRot3.run +true +true +true + + +make +clean +true +true +true + + +make + +tests/testPose2.run +true +true +true + + +make + +tests/testFundamental.run +true +true +true + + +make + +tests/testTensors.run +true +true +true + + +make + +SimpleRotation.run +true +true +true + + +make + +SLAMSelfContained.run +true +true +true + + +make + +PlanarSLAMExample.run +true +true +true + + +make + +check +true +true +true + + +make + +check +true +true +true + + +make +tests/testLieConfig.run +true +true +true + + +make +-j2 +install +true +true +true + + +make +-j2 +check +true +true +true + + +make +clean +true +true +true + + +make +-j2 +install +true +true +true + + +make +-j2 +clean +true +true +true + + +make +-j2 +check +true +true +true + + +make + +all +true +true +true + + +make + +dist +true +true +true + + +make + +testRot3.run +true +true +true + + +make + +testRot2.run +true +true +true + + +make + +testPose3.run +true +true +true + + +make + +timeRot3.run +true +true +true + + +make + +testPose2.run +true +true +true + + +make + +testCal3_S2.run +true +true +true + + +make + +testSimpleCamera.run +true +true +true + + +make + +testHomography2.run +true +true +true + + +make + +testCalibratedCamera.run +true +true +true + + +make + +check +true +true +true + + +make + +clean +true +true +true + + +make + +testPoint2.run +true +true +true + + +make +check +true +true +true + + +make +testGaussianJunctionTree.run +true +true +true + + +make +testGaussianFactorGraph.run +true +true +true + + +make +timeGaussianFactorGraph.run +true +true +true + + +make + +testTupleConfig.run +true +true +true + + +make + +testFusionTupleConfig.run +true +true +true + + +make +testSerialization.run +true +true +true + + +make + +SimpleRotation.run +true +true +true + + +make +check +true +true +true + + +make +clean +true +true +true + + +make +install +true +true +true + + +make +all +true +true +true + + + + - - + + diff --git a/.project b/.project index c10203bc2..893b1174b 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - + -j2 org.eclipse.cdt.make.core.buildCommand diff --git a/geometry/Makefile.am b/geometry/Makefile.am index ee1636780..fc020965e 100644 --- a/geometry/Makefile.am +++ b/geometry/Makefile.am @@ -26,7 +26,7 @@ check_PROGRAMS += tests/testStereoCamera headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h sources += projectiveGeometry.cpp tensorInterface.cpp -check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testTrifocal +check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testTrifocal tests/testFundamental # Timing tests noinst_PROGRAMS = tests/timeRot3 tests/timeCalibratedCamera diff --git a/geometry/tests/testFundamental.cpp b/geometry/tests/testFundamental.cpp new file mode 100644 index 000000000..63a94ecb8 --- /dev/null +++ b/geometry/tests/testFundamental.cpp @@ -0,0 +1,112 @@ +/* + * testFundamental.cpp + * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf + * Created on: Feb 13, 2010 + * @author: Frank Dellaert + */ + +#include +#include +#include // for operator += +using namespace boost::assign; + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace tensors; + +/* ************************************************************************* */ +// Indices + +Index<3, 'a'> a; +Index<3, 'b'> b; + +Index<4, 'A'> A; +Index<4, 'B'> B; + +/* ************************************************************************* */ +TEST( Tensors, FundamentalMatrix) +{ + double f[3][3] = { { 1, 0, 0 }, { 1, 2, 3 }, { 1, 2, 3 } }; + FundamentalMatrix F(f); + + Point2h p = point2h(1, 2, 3); // point p in view one + Point2h q = point2h(14, -1, 0); // point q in view two + + // points p and q are in correspondence + CHECK(F(a,b)*p(a)*q(b) == 0) + + // in detail, l1(b)*q(b)==0 + Line2h l1 = line2h(1, 14, 14); + CHECK(F(a,b)*p(a) == l1(b)) + CHECK(l1(b)*q(b) == 0); // q is on line l1 + + // and l2(a)*p(a)==0 + Line2h l2 = line2h(13, -2, -3); + CHECK(F(a,b)*q(b) == l2(a)) + CHECK(l2(a)*p(a) == 0); // p is on line l2 +} + +/* ************************************************************************* */ +// Stereo setup, -1,1 +/* ************************************************************************* */ +// t points towards origin +double left__[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { +1, 0, 0 } }; +double right_[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { -1, 0, 0 } }; +//double right_[4][3] = { { cos(0.1), -sin(0.1), 0 }, { sin(0.1), cos(0.1), 0 }, { 0, 0, 1 }, { -1, 0, 0 } }; +ProjectiveCamera ML(left__), MR(right_); + +// Cube +Point3h P1 = point3h(-1, -1, 3 - 1, 1); +Point3h P2 = point3h(-1, -1, 3 + 1, 1); +Point3h P3 = point3h(-1, +1, 3 - 1, 1); +Point3h P4 = point3h(-1, +1, 3 + 1, 1); +Point3h P5 = point3h(+1, -1, 3 - 1, 1); +Point3h P6 = point3h(+1, -1, 3 + 1, 1); +Point3h P7 = point3h(+1, +1, 3 - 1, 1); +Point3h P8 = point3h(+1, +1, 3 + 1, 1); + +/* ************************************************************************* */ +//TEST( Tensors, FundamentalMatrix2) +//{ +// // The matrix A is rank-deficient, but as checked below this one +// // out of many in the zero-error subspace is a correct F for the rig +// double f[3][3] = {{-0, 5.06764, -0.840876} +// ,{-4.62947, 1.25142, 1.99583e+16} +// ,{-1.70847, -1.99583e+16, 1} +// }; +// FundamentalMatrix F(f); +// +// list points; +// Point3h P9 = point3h(-2,3,4,1); +// Point3h P10 = point3h(1,1,5,1); +// points += P1, P2, P3, P4, P5, P6, P7, P8, P9, P10; +// list correspondences; +// BOOST_FOREACH(const Point3h& P, points) { +// print(P(A)); +// Correspondence p(ML(a,A)*P(A), MR(b,A)*P(A)); +// print(ML(a,A)*P(A));print(MR(b,A)*P(A)); +//// DOUBLES_EQUAL(0,F(a,b) * p.first(a) * p.second(b),1e-9); // checked here for cube +// correspondences += p; +// } +// +// // let's check it for another arbitrary point +// Point2h left(ML(a,A)*P9(A)), right(MR(b,A)*P9(A)); +//// DOUBLES_EQUAL(0,F(a,b) * left(a) * right(b),1e-9); +// +// FundamentalMatrix actual = estimateFundamentalMatrix(correspondences); +// CHECK(assert_equality(F(a,b),actual(a,b)*(1.0/actual(2,2)),0.1)); +//} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +