Added a FixedVector class that uses a bounded boost vector to allow for type-level dimension specification
parent
71074b2188
commit
99e4c09a7e
|
@ -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 <Testable.h>
|
||||
#include <stdarg.h>
|
||||
#include <Vector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Fixed size vectors - compatible with boost vectors, but with compile-type
|
||||
* size checking.
|
||||
*/
|
||||
template<size_t N>
|
||||
class FixedVector : public boost::numeric::ublas::bounded_vector<double, N> ,
|
||||
public Testable<FixedVector<N> > {
|
||||
public:
|
||||
typedef boost::numeric::ublas::bounded_vector<double, N> 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<N; ++i)
|
||||
v(i) = value;
|
||||
return v;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create basis vector of
|
||||
* with a constant in spot i
|
||||
* @param index of the one
|
||||
* @param value is the value to insert into the vector
|
||||
* @return delta vector
|
||||
*/
|
||||
inline static FixedVector delta(size_t i, double value) {
|
||||
FixedVector v = zero();
|
||||
v(i) = value;
|
||||
return v;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create basis vector,
|
||||
* with one in spot i
|
||||
* @param index of the one
|
||||
* @return basis vector
|
||||
*/
|
||||
inline static FixedVector basis(size_t i) { return delta(i, 1.0); }
|
||||
|
||||
/**
|
||||
* Create zero vector
|
||||
*/
|
||||
inline static FixedVector zero() { return repeat(0.0);}
|
||||
|
||||
/**
|
||||
* Create vector initialized to ones
|
||||
*/
|
||||
inline static FixedVector ones() { return repeat(1.0);}
|
||||
|
||||
static size_t dim() { return Base::max_size; }
|
||||
|
||||
void print(const std::string& name="") const { gtsam::print(Vector(*this), name); }
|
||||
|
||||
template<size_t M>
|
||||
bool equals(const FixedVector<M>& 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
|
|
@ -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
|
||||
|
|
|
@ -0,0 +1,76 @@
|
|||
/**
|
||||
* @file testFixedVector.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <FixedVector.h>
|
||||
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
Loading…
Reference in New Issue