12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

VectorValues documentation and interface cleanup, additional unit tests and bug fix.

release/4.3a0
Richard Roberts 2011-10-29 18:53:08 +00:00
parent 9ff18b4e4e
commit eb8fb31b2a
4 changed files with 242 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/linear/VectorValues.cpp
View File

@ -121,8 +121,17 @@ VectorValues VectorValues::SameStructure(const VectorValues& other) {
return ret;
}
/* ************************************************************************* */
VectorValues VectorValues::Zero(Index nVars, size_t varDim) {
VectorValues ret(nVars, varDim);
ret.vector() = Vector::Zero(ret.dim());
return ret;
}
/* ************************************************************************* */
bool VectorValues::hasSameStructure(const VectorValues& other) const {
if(this->size() != other.size())
return false;
for(size_t j=0; j<size(); ++j)
if(this->dim(j) != other.dim(j))
return false;

129
gtsam/linear/VectorValues.h
View File

@ -27,8 +27,8 @@
namespace gtsam {
/**
* This class stores a collection of vector-valued variables, each referenced
* by a unique variable index. It is typically used to store the variables
* This class represents a collection of vector-valued variables associated
* each with a unique integer index. It is typically used to store the variables
* of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet
* returns this class.
*
@ -41,11 +41,16 @@ namespace gtsam {
* - \ref exists (Index) to check if a variable is present
* - Other facilities like iterators, size(), dim(), etc.
*
* Indices can be non-consecutive and inserted out-of-order, but you should not
* use indices that are larger than a reasonable array size because the indices
* correspond to positions in an internal array.
*
* Example:
* \code
VectorValues values;
values.insert(0, Vector_(3, 5.0, 6.0, 7.0));
values.insert(1, Vector_(2, 3.0, 4.0));
values.insert(3, Vector_(3, 1.0, 2.0, 3.0));
values.insert(4, Vector_(2, 4.0, 5.0));
values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0));
// Prints [ 3.0 4.0 ]
gtsam::print(values[1]);
@ -55,15 +60,24 @@ namespace gtsam {
gtsam::print(values[1]);
\endcode
*
* Internally, this class stores all vectors as part of one large vector. This is
* necessary for performance, and the gtsam linear solving code exploits it by
* only allocating one large vector to store the solution. For advanced usage,
* or where speed is important, be aware of the following:
* - It is faster to allocate space ahead of time using a pre-allocating constructor
* or the resize() and append() functions, than to use insert(Index, const Vector&),
* which always has to re-allocate the internal vector.
* - The vector() function permits access to the underlying Vector.
* - operator[]() returns a SubVector view of the underlying Vector.
* <h2>Advanced Interface and Performance Information</h2>
*
* Internally, all vector values are stored as part of one large vector. In
* gtsam this vector is always pre-allocated for efficiency, using the
* advanced interface described below. Accessing and modifying already-allocated
* values is \f$ O(1) \f$. Using the insert() function of the standard interface
* is slow because it requires re-allocating the internal vector.
*
* For advanced usage, or where speed is important:
* - Allocate space ahead of time using a pre-allocating constructor
* (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
* SameStructure(), resize(), or append(). Do not use
* insert(Index, const Vector&), which always has to re-allocate the
* internal vector.
* - The vector() function permits access to the underlying Vector, for
* doing mathematical or other operations that require all values.
* - operator[]() returns a SubVector view of the underlying Vector,
* without copying any data.
*
* Access is through the variable index j, and returns a SubVector,
* which is a view on the underlying data structure.
@ -84,16 +98,16 @@ namespace gtsam {
typedef ValueMaps::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
typedef boost::shared_ptr<VectorValues> shared_ptr; ///< shared_ptr to this class
/// @name Standard constructors
/// @name Standard Constructors
/// @{
/**
* Default constructor creates an empty VectorValues.
*/
VectorValues() {} //
VectorValues() {}
/** Copy constructor */
VectorValues(const VectorValues &other); //
VectorValues(const VectorValues &other);
/** Named constructor to create a VectorValues of the same structure of the
* specifed one, but filled with zeros.
@ -102,27 +116,33 @@ namespace gtsam {
static VectorValues Zero(const VectorValues& model);
/// @}
/// @name Standard interface
/// @name Standard Interface
/// @{
/** Number of variables stored, always 1 more than the highest variable index,
* even if some variables with lower indices are not present. */
Index size() const { return maps_.size(); } //
Index size() const { return maps_.size(); }
/** Return the dimension of variable \c j. */
size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } //
size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
/** Return the summed dimensionality of all variables. */
size_t dim() const { return values_.rows(); } //
size_t dim() const { return values_.rows(); }
/** Check whether a variable exists by index. */
bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } //
/** Check whether a variable with index \c j exists. */
bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; }
/** Reference a variable by index. */
SubVector& operator[](Index j) { checkExists(j); return maps_[j]; } //
/** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
SubVector& at(Index j) { checkExists(j); return maps_[j]; }
/** Reference a variable by index. */
const SubVector& operator[](Index j) const { checkExists(j); return maps_[j]; } //
/** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
const SubVector& at(Index j) const { checkExists(j); return maps_[j]; }
/** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */
SubVector& operator[](Index j) { return at(j); }
/** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */
const SubVector& operator[](Index j) const { return at(j); }
/** Insert a vector \c value with index \c j.
* Causes reallocation. Can be used to insert values in any order, but
@ -130,68 +150,79 @@ namespace gtsam {
* @param value The vector to be inserted.
* @param j The index with which the value will be associated.
*/
void insert(Index j, const Vector& value); //
void insert(Index j, const Vector& value);
/** Assignment */
VectorValues& operator=(const VectorValues& rhs); //
VectorValues& operator=(const VectorValues& rhs);
iterator begin() { chk(); return maps_.begin(); } ///< Iterator over variables
const_iterator begin() const { chk(); return maps_.begin(); } ///< Iterator over variables
iterator end() { chk(); return maps_.end(); } ///< Iterator over variables
const_iterator end() const { chk(); return maps_.end(); } ///< Iterator over variables
reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Iterator over variables
const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Iterator over variables
reverse_iterator rend() { chk(); return maps_.rend(); } ///< Iterator over variables
const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Iterator over variables
reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables
const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const; //
void print(const std::string& str = "VectorValues: ") const;
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol = 1e-9) const; //
bool equals(const VectorValues& x, double tol = 1e-9) const;
/// @}
/// @name Advanced constructors
/// \anchor AdvancedConstructors
/// @name Advanced Constructors
/// @{
/** Construct from a container of variable dimensions (in variable order). */
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions) { append(dimensions); } //
VectorValues(const CONTAINER& dimensions) { append(dimensions); }
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); } //
VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); }
/** Named constructor to create a VectorValues that matches the structure of
* the specified VectorValues, but do not initialize the new values. */
static VectorValues SameStructure(const VectorValues& other); //
static VectorValues SameStructure(const VectorValues& other);
/** Named constructor to create a VectorValues from a container of variable
* dimensions that is filled with zeros. */
* dimensions that is filled with zeros.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
static VectorValues Zero(const CONTAINER& dimensions);
/** Named constructor to create a VectorValues filled with zeros that has
* \c nVars variables, each of dimension \c varDim
* @param nVars The number of variables to create
* @param varDim The dimension of each variable
* @return The new VectorValues
*/
static VectorValues Zero(Index nVars, size_t varDim);
/// @}
/// @name Advanced interface
/// @name Advanced Interface
/// @{
/** Resize this VectorValues to have identical structure to other, leaving
* this VectorValues with uninitialized values.
* @param other The VectorValues whose structure to copy
*/
void resizeLike(const VectorValues& other); //
void resizeLike(const VectorValues& other);
/** Resize the VectorValues to hold \c nVars variables, each of dimension
* \c varDim. This function does not preserve any data, after calling
* it all variables will be uninitialized.
* \c varDim, not preserving any data. After calling this function, all
* variables will be uninitialized.
* @param nVars The number of variables to create
* @param varDim The dimension of each variable
*/
void resize(Index nVars, size_t varDim);
/** Resize the VectorValues to contain variables of the dimensions stored
* in \c dimensions. The new variables are uninitialized, but this function
* is used to pre-allocate space for performance. This function does not
* preserve any data, after calling it all variables will be uninitialized.
* in \c dimensions, not preserving any data. The new variables are
* uninitialized, but this function is used to pre-allocate space for
* performance. After calling this function all variables will be uninitialized.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
@ -205,13 +236,13 @@ namespace gtsam {
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
void append(const CONTAINER& dimensions); //
void append(const CONTAINER& dimensions);
/** Reference the entire solution vector (const version). */
const Vector& vector() const { chk(); return values_; } //
const Vector& vector() const { chk(); return values_; }
/** Reference the entire solution vector. */
Vector& vector() { chk(); return values_; } //
Vector& vector() { chk(); return values_; }
/** Check whether this VectorValues has the same structure, meaning has the
* same number of variables and that all variables are of the same dimension,

196
gtsam/linear/tests/testVectorValues.cpp
View File

@ -135,14 +135,17 @@ TEST(VectorValues, copyConstructor) {
/* ************************************************************************* */
TEST(VectorValues, assignment) {
VectorValues actual;
{
// insert, with out-of-order indices
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValues actual = original;
actual = original;
}
// Check dimensions
LONGS_EQUAL(6, actual.size());
@ -204,6 +207,83 @@ TEST(VectorValues, SameStructure) {
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, Zero_fromModel) {
// insert, with out-of-order indices
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValues actual(VectorValues::Zero(original));
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Values
EXPECT(assert_equal(Vector::Zero(1), actual[0]));
EXPECT(assert_equal(Vector::Zero(2), actual[1]));
EXPECT(assert_equal(Vector::Zero(2), actual[5]));
EXPECT(assert_equal(Vector::Zero(2), actual[2]));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, Zero_fromDims) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
VectorValues actual(VectorValues::Zero(dims));
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(5, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Values
EXPECT(assert_equal(Vector::Zero(1), actual[0]));
EXPECT(assert_equal(Vector::Zero(2), actual[1]));
EXPECT(assert_equal(Vector::Zero(2), actual[2]));
}
/* ************************************************************************* */
TEST(VectorValues, Zero_fromUniform) {
VectorValues actual(VectorValues::Zero(3, 2));
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(6, actual.dim());
LONGS_EQUAL(2, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Values
EXPECT(assert_equal(Vector::Zero(2), actual[0]));
EXPECT(assert_equal(Vector::Zero(2), actual[1]));
EXPECT(assert_equal(Vector::Zero(2), actual[2]));
}
/* ************************************************************************* */
TEST(VectorValues, resizeLike) {
// insert, with out-of-order indices
@ -237,6 +317,56 @@ TEST(VectorValues, resizeLike) {
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, resize_fromUniform) {
VectorValues actual(4, 10);
actual.resize(3, 2);
actual[0] = Vector_(2, 1.0, 2.0);
actual[1] = Vector_(2, 2.0, 3.0);
actual[2] = Vector_(2, 4.0, 5.0);
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(6, actual.dim());
LONGS_EQUAL(2, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Check values
EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
}
/* ************************************************************************* */
TEST(VectorValues, resize_fromDims) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
VectorValues actual(4, 10);
actual.resize(dims);
actual[0] = Vector_(1, 1.0);
actual[1] = Vector_(2, 2.0, 3.0);
actual[2] = Vector_(2, 4.0, 5.0);
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(5, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
}
/* ************************************************************************* */
TEST(VectorValues, append) {
// insert
@ -277,6 +407,20 @@ TEST(VectorValues, append) {
CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, hasSameStructure) {
VectorValues v1(2, 3);
VectorValues v2(3, 2);
VectorValues v3(4, 2);
VectorValues v4(4, 2);
EXPECT(!v1.hasSameStructure(v2));
EXPECT(!v2.hasSameStructure(v3));
EXPECT(v3.hasSameStructure(v4));
EXPECT(VectorValues().hasSameStructure(VectorValues()));
EXPECT(!v1.hasSameStructure(VectorValues()));
}
/* ************************************************************************* */
TEST(VectorValues, permuted_combined) {
Vector v1 = Vector_(3, 1.0,2.0,3.0);
@ -323,56 +467,8 @@ TEST(VectorValues, permuted_combined) {
CHECK(assert_equal(v1, permuted2[1]))
CHECK(assert_equal(v2, permuted2[2]))
CHECK(assert_equal(v3, permuted2[0]))
}
///* ************************************************************************* */
//TEST(VectorValues, range ) {
// VectorValues v(7,2);
// v.makeZero();
// v[1] = Vector_(2, 1.0, 2.0);
// v[2] = Vector_(2, 3.0, 4.0);
// v[3] = Vector_(2, 5.0, 6.0);
//
// vector<size_t> idx1, idx2;
// idx1 += 0, 1, 2, 3, 4, 5, 6; // ordered
// idx2 += 1, 0, 2; // unordered
//
// // test access
//
// Vector actRange1 = v.range(idx1.begin(), idx1.begin() + 2);
// EXPECT(assert_equal(Vector_(4, 0.0, 0.0, 1.0, 2.0), actRange1));
//
// Vector actRange2 = v.range(idx1.begin()+1, idx1.begin()+2);
// EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actRange2));
//
// Vector actRange3 = v.range(idx2.begin(), idx2.end());
// EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 0.0, 0.0, 3.0, 4.0), actRange3));
//
// // test setting values
// VectorValues act1 = v, act2 = v, act3 = v;
//
// Vector a = Vector_(2, 0.1, 0.2);
// VectorValues exp1 = act1; exp1[0] = a;
// act1.range(idx1.begin(), idx1.begin()+1, a);
// EXPECT(assert_equal(exp1, act1));
//
// Vector bc = Vector_(4, 0.1, 0.2, 0.3, 0.4);
// VectorValues exp2 = act2;
// exp2[2] = Vector_(2, 0.1, 0.2);
// exp2[3] = Vector_(2, 0.3, 0.4);
// act2.range(idx1.begin()+2, idx1.begin()+4, bc);
// EXPECT(assert_equal(exp2, act2));
//
// Vector def = Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
// VectorValues exp3 = act3;
// exp3[1] = Vector_(2, 0.1, 0.2);
// exp3[0] = Vector_(2, 0.3, 0.4);
// exp3[2] = Vector_(2, 0.5, 0.6);
// act3.range(idx2.begin(), idx2.end(), def);
// EXPECT(assert_equal(exp3, act3));
//}
/* ************************************************************************* */
int main() {
TestResult tr; return TestRegistry::runAllTests(tr);

4
tests/testGaussianISAM.cpp
View File

@ -70,7 +70,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
EXPECT(assert_equal(expected, actual));
// obtain solution
VectorValues e(VectorValues::Zero(vector<size_t>(7,2))); // expected solution
VectorValues e(VectorValues::Zero(7,2)); // expected solution
VectorValues optimized = optimize(actual); // actual solution
EXPECT(assert_equal(e, optimized));
}
@ -181,7 +181,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
// Create the Bayes tree
BayesTree<GaussianConditional> chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate();
VectorValues expectedSolution(VectorValues::Zero(vector<size_t>(7,2)));
VectorValues expectedSolution(VectorValues::Zero(7,2));
VectorValues actualSolution = optimize(chordalBayesNet);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));