From 99e4c09a7ea18a4faf25c17565537e97236312c2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 2 Aug 2010 19:01:00 +0000 Subject: [PATCH] Added a FixedVector class that uses a bounded boost vector to allow for type-level dimension specification --- .cproject | 370 ++++++++++++++++++--------------- base/FixedVector.h | 114 ++++++++++ base/Makefile.am | 4 +- base/tests/testFixedVector.cpp | 76 +++++++ 4 files changed, 395 insertions(+), 169 deletions(-) create mode 100644 base/FixedVector.h create mode 100644 base/tests/testFixedVector.cpp diff --git a/.cproject b/.cproject index f2cb2c0fa..ebbb1f90c 100644 --- a/.cproject +++ b/.cproject @@ -302,7 +302,6 @@ make - all true true @@ -310,7 +309,6 @@ make - clean true true @@ -318,7 +316,6 @@ make - check true true @@ -326,7 +323,6 @@ make - testGaussianConditional.run true true @@ -334,7 +330,6 @@ make - testGaussianFactor.run true true @@ -342,7 +337,6 @@ make - timeGaussianFactor.run true true @@ -350,7 +344,6 @@ make - timeVectorConfig.run true true @@ -358,7 +351,6 @@ make - testVectorBTree.run true true @@ -366,7 +358,6 @@ make - testVectorMap.run true true @@ -374,7 +365,6 @@ make - testNoiseModel.run true true @@ -382,7 +372,6 @@ make - testBayesNetPreconditioner.run true true @@ -390,7 +379,6 @@ make - testErrors.run true false @@ -398,6 +386,7 @@ make + check true true @@ -405,14 +394,22 @@ make + tests/testSPQRUtil.run true true true + + make + + clean + true + true + true + make - check true true @@ -420,7 +417,6 @@ make - tests/testGaussianJunctionTree.run true true @@ -428,6 +424,7 @@ make + check true true @@ -435,6 +432,7 @@ make + clean true true @@ -442,6 +440,7 @@ make + testBTree.run true true @@ -449,6 +448,7 @@ make + testDSF.run true true @@ -456,6 +456,7 @@ make + testDSFVector.run true true @@ -463,6 +464,7 @@ make + testMatrix.run true true @@ -470,6 +472,7 @@ make + testSPQRUtil.run true true @@ -477,6 +480,7 @@ make + testVector.run true true @@ -484,6 +488,7 @@ make + timeMatrix.run true true @@ -491,6 +496,7 @@ make + all true true @@ -498,7 +504,6 @@ make - all true true @@ -506,7 +511,6 @@ make - clean true true @@ -522,7 +526,6 @@ make - testBayesTree.run true false @@ -530,7 +533,6 @@ make - testBinaryBayesNet.run true false @@ -538,7 +540,6 @@ make - testFactorGraph.run true true @@ -546,7 +547,6 @@ make - testISAM.run true true @@ -554,7 +554,6 @@ make - testJunctionTree.run true true @@ -562,7 +561,6 @@ make - testKey.run true true @@ -570,7 +568,6 @@ make - testOrdering.run true true @@ -578,7 +575,6 @@ make - testSymbolicBayesNet.run true false @@ -586,7 +582,6 @@ make - testSymbolicFactor.run true false @@ -594,7 +589,6 @@ make - testSymbolicFactorGraph.run true false @@ -602,7 +596,6 @@ make - timeSymbolMaps.run true true @@ -610,7 +603,6 @@ make - check true true @@ -618,7 +610,6 @@ make - testClusterTree.run true true @@ -626,7 +617,6 @@ make - testJunctionTree.run true true @@ -634,7 +624,6 @@ make - testEliminationTree.run true true @@ -642,6 +631,7 @@ make + check true true @@ -649,6 +639,7 @@ make + testGaussianFactorGraph.run true true @@ -656,6 +647,7 @@ make + testGaussianISAM.run true true @@ -663,6 +655,7 @@ make + testGaussianISAM2.run true true @@ -670,6 +663,7 @@ make + testGraph.run true false @@ -677,6 +671,7 @@ make + testIterative.run true true @@ -684,6 +679,7 @@ make + testNonlinearEquality.run true true @@ -691,6 +687,7 @@ make + testNonlinearFactor.run true true @@ -698,6 +695,7 @@ make + testNonlinearFactorGraph.run true true @@ -705,6 +703,7 @@ make + testNonlinearOptimizer.run true true @@ -712,6 +711,7 @@ make + testSQP.run true true @@ -719,6 +719,7 @@ make + testSubgraphPreconditioner.run true true @@ -726,6 +727,7 @@ make + testTupleConfig.run true true @@ -733,6 +735,7 @@ make + timeGaussianFactorGraph.run true true @@ -740,6 +743,7 @@ make + testBayesNetPreconditioner.run true true @@ -747,6 +751,7 @@ make + testConstraintOptimizer.run true true @@ -754,6 +759,7 @@ make + testInference.run true false @@ -761,6 +767,7 @@ make + testGaussianBayesNet.run true false @@ -768,6 +775,7 @@ make + testGaussianFactor.run true false @@ -775,6 +783,7 @@ make + testJunctionTree.run true false @@ -782,6 +791,7 @@ make + testSymbolicBayesNet.run true false @@ -789,6 +799,7 @@ make + testSymbolicFactorGraph.run true false @@ -796,7 +807,6 @@ make - clean true true @@ -804,7 +814,6 @@ make - all true true @@ -812,7 +821,6 @@ make - testNonlinearConstraint.run true true @@ -820,7 +828,6 @@ make - testLieConfig.run true true @@ -828,7 +835,6 @@ make - testConstraintOptimizer.run true true @@ -836,7 +842,6 @@ make - install true true @@ -844,7 +849,6 @@ make - clean true true @@ -852,6 +856,7 @@ make + all true true @@ -859,6 +864,7 @@ make + clean true true @@ -866,6 +872,7 @@ make + all true true @@ -873,6 +880,7 @@ make + clean true true @@ -880,6 +888,7 @@ make + all true true @@ -887,6 +896,7 @@ make + clean true true @@ -894,7 +904,6 @@ make - all true true @@ -902,7 +911,6 @@ make - clean true true @@ -910,6 +918,7 @@ make + clean all true true @@ -917,6 +926,7 @@ make + all true true @@ -924,6 +934,7 @@ make + check true true @@ -931,6 +942,7 @@ make + clean true true @@ -938,6 +950,7 @@ make + testPlanarSLAM.run true true @@ -945,6 +958,7 @@ make + testPose2Config.run true true @@ -952,6 +966,7 @@ make + testPose2Factor.run true true @@ -959,6 +974,7 @@ make + testPose2Prior.run true true @@ -966,6 +982,7 @@ make + testPose2SLAM.run true true @@ -973,6 +990,7 @@ make + testPose3Config.run true true @@ -980,6 +998,7 @@ make + testPose3SLAM.run true true @@ -987,6 +1006,7 @@ make + testSimulated2DOriented.run true false @@ -994,6 +1014,7 @@ make + testVSLAMConfig.run true true @@ -1001,6 +1022,7 @@ make + testVSLAMFactor.run true true @@ -1008,6 +1030,7 @@ make + testVSLAMGraph.run true true @@ -1015,6 +1038,7 @@ make + testPose3Factor.run true true @@ -1022,6 +1046,7 @@ make + testSimulated2D.run true false @@ -1029,6 +1054,7 @@ make + testSimulated3D.run true false @@ -1036,6 +1062,7 @@ make + check true true @@ -1043,6 +1070,15 @@ make + + check + true + true + true + + + make + check true true @@ -1066,103 +1102,11 @@ make - clean true true true - - make - check - 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 @@ -1189,6 +1133,7 @@ make + all true true @@ -1196,12 +1141,85 @@ 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 @@ -1209,39 +1227,7 @@ true true - - make - - testGaussianJunctionTree.run - true - true - true - - - make - - testGaussianFactorGraph.run - true - true - true - - - make - - timeGaussianFactorGraph.run - true - true - true - - - make - - check - true - true - true - - + make clean @@ -1249,9 +1235,58 @@ 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 + check + true + true + true + + + make + clean + true + true + true + + + make install true true @@ -1259,7 +1294,6 @@ make - all true true diff --git a/base/FixedVector.h b/base/FixedVector.h new file mode 100644 index 000000000..164b1acd9 --- /dev/null +++ b/base/FixedVector.h @@ -0,0 +1,114 @@ +/** + * @file FixedVector.h + * @brief Extension of boost's bounded_vector to allow for fixed size operation + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Fixed size vectors - compatible with boost vectors, but with compile-type + * size checking. + */ +template +class FixedVector : public boost::numeric::ublas::bounded_vector , + public Testable > { +public: + typedef boost::numeric::ublas::bounded_vector Base; + + /** default constructor */ + FixedVector() {} + + /** copy constructors */ + FixedVector(const FixedVector& v) : Base(v) {} + + /** Convert from a variable-size vector to a fixed size vector */ + FixedVector(const Vector& v) : Base(v) {} + + /** Initialize with a C-style array */ + FixedVector(const double* values) { + std::copy(values, values+N, this->data().begin()); + } + + /** + * nice constructor, dangerous as number of arguments must be exactly right + * and you have to pass doubles !!! always use 0.0 never 0 + * + * NOTE: this will throw warnings/explode if there is no argument + * before the variadic section, so there is a meaningless size argument. + */ + FixedVector(size_t n, ...) { + va_list ap; + va_start(ap, n); + for(size_t i = 0 ; i < N ; i++) { + double value = va_arg(ap, double); + (*this)(i) = value; + } + va_end(ap); + } + + /** + * Create vector initialized to a constant value + * @param constant value + */ + inline static FixedVector repeat(double value) { + FixedVector v; + for (size_t i=0; i + bool equals(const FixedVector& other, double tol=1e-9) const { + return false; + } + + bool equals(const FixedVector& other, double tol=1e-9) const { + return equal_with_abs_tol(*this,other,tol); + } + +}; + + +} // \namespace diff --git a/base/Makefile.am b/base/Makefile.am index c99daabd2..22619d1fd 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -8,8 +8,10 @@ sources = check_PROGRAMS = # base Math + +headers += FixedVector.h sources += Vector.cpp svdcmp.cpp Matrix.cpp -check_PROGRAMS += tests/testVector tests/testMatrix +check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix if USE_LAPACK sources += SPQRUtil.cpp diff --git a/base/tests/testFixedVector.cpp b/base/tests/testFixedVector.cpp new file mode 100644 index 000000000..567859a0c --- /dev/null +++ b/base/tests/testFixedVector.cpp @@ -0,0 +1,76 @@ +/** + * @file testFixedVector.cpp + * @author Alex Cunningham + */ + +#include + +#include + +using namespace gtsam; + +typedef FixedVector<5> Vector5; +typedef FixedVector<3> Vector3; + +static const double tol = 1e-9; + +/* ************************************************************************* */ +TEST( testFixedVector, conversions ) { + double data1[] = {1.0, 2.0, 3.0}; + Vector v1 = Vector_(3, data1); + Vector3 fv1(v1), fv2(data1); + + Vector actFv2(fv2); + CHECK(assert_equal(v1, actFv2)); +} + +/* ************************************************************************* */ +TEST( testFixedVector, variable_constructor ) { + Vector3 act(3, 1.0, 2.0, 3.0); + DOUBLES_EQUAL(1.0, act(0), tol); + DOUBLES_EQUAL(2.0, act(1), tol); + DOUBLES_EQUAL(3.0, act(2), tol); +} + +/* ************************************************************************* */ +TEST( testFixedVector, equals ) { + Vector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0); + Vector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0); + + CHECK(assert_equal(vec1, vec1, tol)); + CHECK(assert_equal(vec1, vec2, tol)); + CHECK(assert_equal(vec2, vec1, tol)); + CHECK(!vec1.equals(vec3, tol)); + CHECK(!vec3.equals(vec1, tol)); + CHECK(!vec1.equals(vec4, tol)); + CHECK(!vec4.equals(vec1, tol)); +} + +/* ************************************************************************* */ +TEST( testFixedVector, static_constructors ) { + Vector3 actZero = Vector3::zero(); + Vector3 expZero(3, 0.0, 0.0, 0.0); + CHECK(assert_equal(expZero, actZero, tol)); + + Vector3 actOnes = Vector3::ones(); + Vector3 expOnes(3, 1.0, 1.0, 1.0); + CHECK(assert_equal(expOnes, actOnes, tol)); + + Vector3 actRepeat = Vector3::repeat(2.3); + Vector3 expRepeat(3, 2.3, 2.3, 2.3); + CHECK(assert_equal(expRepeat, actRepeat, tol)); + + Vector3 actBasis = Vector3::basis(1); + Vector3 expBasis(3, 0.0, 1.0, 0.0); + CHECK(assert_equal(expBasis, actBasis, tol)); + + Vector3 actDelta = Vector3::delta(1, 2.3); + Vector3 expDelta(3, 0.0, 2.3, 0.0); + CHECK(assert_equal(expDelta, actDelta, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +