VectorValues documentation and interface cleanup, additional unit tests and bug fix.
Richard Roberts
parent
9ff18b4e4e
commit
eb8fb31b2a
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -135,14 +135,17 @@ TEST(VectorValues, copyConstructor) {
|
|||
/* ************************************************************************* */
|
||||
TEST(VectorValues, assignment) {
|
||||
|
||||
// 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 actual = original;
|
||||
{
|
||||
// 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));
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue