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

Unit tests pass with piecewise VectorValues implementation (for fast permutations)

release/4.3a0
Richard Roberts 2012-12-18 14:21:02 +00:00
parent c7b9345aa1
commit 7309aa0ffa
15 changed files with 215 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -607,11 +607,11 @@ break;
/* ************************************************************************* */
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.asVector() = Vector::Zero(r.dim());
r.setZero();
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
SubVector &y = r[i];
Vector &y = r[i];
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j];
}
@ -621,7 +621,7 @@ break;
/* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.asVector() = Vector::Zero(x.dim());
x.setZero();
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);

153
gtsam/linear/VectorValues.cpp
View File

@ -20,6 +20,8 @@
#include <gtsam/inference/Permutation.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/iterator/counting_iterator.hpp>
using namespace std;
namespace gtsam {
@ -29,13 +31,13 @@ VectorValues VectorValues::Zero(const VectorValues& x) {
VectorValues result;
result.values_.resize(x.size());
for(size_t j=0; j<x.size(); ++j)
result.values_[j] = Vector::Zero(x.dim(j));
result.values_[j] = Vector::Zero(x.values_[j].size());
return result;
}
/* ************************************************************************* */
vector<size_t> VectorValues::dims() const {
vector<size_t> result(this->size());
std::vector<size_t> result(this->size());
for(Index j = 0; j < this->size(); ++j)
result[j] = this->dim(j);
return result;
@ -67,7 +69,7 @@ void VectorValues::print(const std::string& str, const IndexFormatter& formatter
bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size())
return false;
for(size_t j=0; j<size(); ++j)
for(Index j=0; j < size(); ++j)
if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
return false;
return true;
@ -75,27 +77,16 @@ bool VectorValues::equals(const VectorValues& x, double tol) const {
/* ************************************************************************* */
void VectorValues::resize(Index nVars, size_t varDim) {
maps_.clear();
maps_.reserve(nVars);
values_.resize(nVars * varDim);
int varStart = 0;
for (Index j = 0; j < nVars; ++j) {
maps_.push_back(values_.segment(varStart, varDim));
varStart += varDim;
}
values_.resize(nVars);
for(Index j = 0; j < nVars; ++j)
values_[j] = Vector(varDim);
}
/* ************************************************************************* */
void VectorValues::resizeLike(const VectorValues& other) {
values_.resize(other.dim());
// Create SubVectors referencing our values_ vector
maps_.clear();
maps_.reserve(other.size());
int varStart = 0;
BOOST_FOREACH(const SubVector& value, other) {
maps_.push_back(values_.segment(varStart, value.rows()));
varStart += value.rows();
}
values_.resize(other.size());
for(Index j = 0; j < other.size(); ++j)
values_[j].resize(other.values_[j].size());
}
/* ************************************************************************* */
@ -114,99 +105,117 @@ VectorValues VectorValues::Zero(Index nVars, size_t varDim) {
/* ************************************************************************* */
void VectorValues::setZero() {
values_.setZero();
BOOST_FOREACH(Vector& v, *this) {
v.setZero();
}
}
/* ************************************************************************* */
const Vector VectorValues::asVector() const {
return internal::extractVectorValuesSlices(*this,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
}
/* ************************************************************************* */
const Vector VectorValues::vector(const std::vector<Index>& indices) const {
return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
}
/* ************************************************************************* */
bool VectorValues::hasSameStructure(const VectorValues& other) const {
if(this->size() != other.size())
return false;
for(size_t j=0; j<size(); ++j)
for(size_t j = 0; j < size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->maps_[j].rows() != other.maps_[j].rows())
if(this->values_[j].rows() != other.values_[j].rows())
return false;
return true;
}
/* ************************************************************************* */
VectorValues VectorValues::permute(const Permutation& permutation) const {
// Create result and allocate space
VectorValues lhs;
lhs.values_.resize(this->dim());
lhs.maps_.reserve(this->size());
void VectorValues::permuteInPlace(const Permutation& permutation) {
// Allocate new values
Values newValues(this->size());
// Copy values from this VectorValues to the permuted VectorValues
size_t lhsPos = 0;
for(size_t i = 0; i < this->size(); ++i) {
// Map the next LHS subvector to the next slice of the LHS vector
lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
// Copy the data from the RHS subvector to the LHS subvector
lhs.maps_[i] = this->at(permutation[i]);
// Increment lhs position
lhsPos += lhs.maps_[i].size();
}
// Swap values from this VectorValues to the permuted VectorValues
for(size_t i = 0; i < this->size(); ++i)
newValues[i].swap(this->at(permutation[i]));
return lhs;
// Swap the values into this VectorValues
values_.swap(newValues);
}
/* ************************************************************************* */
void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_);
this->maps_.swap(other.maps_);
}
/* ************************************************************************* */
double VectorValues::dot(const VectorValues& V) const {
double VectorValues::dot(const VectorValues& v) const {
double result = 0.0;
if(this->size() != v.size())
throw invalid_argument("VectorValues::dot called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == v.values_[j].size())
result += this->values_[j].dot(v.values_[j]);
else
throw invalid_argument("VectorValues::dot called with different vector sizes");
return result;
}
/* ************************************************************************* */
double VectorValues::norm() const {
return std::sqrt(this->squaredNorm());
}
/* ************************************************************************* */
double VectorValues::squaredNorm() const {
double sumSquares = 0.0;
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
sumSquares += this->values_[j].squaredNorm();
return sumSquares;
}
/* ************************************************************************* */
VectorValues VectorValues::operator+(const VectorValues& c) const {
assert(this->hasSameStructure(c));
VectorValues result(SameStructure(c));
result.values_ = this->values_ + c.values_;
VectorValues result = SameStructure(*this);
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] + c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
return result;
}
/* ************************************************************************* */
VectorValues VectorValues::operator-(const VectorValues& c) const {
assert(this->hasSameStructure(c));
VectorValues result(SameStructure(c));
result.values_ = this->values_ - c.values_;
VectorValues result = SameStructure(*this);
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] - c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
return result;
}
/* ************************************************************************* */
void VectorValues::operator+=(const VectorValues& c) {
assert(this->hasSameStructure(c));
this->values_ += c.values_;
}
/* ************************************************************************* */
Vector VectorValues::vector(const std::vector<Index>& indices) const {
if (indices.empty())
return Vector();
// find dimensions
size_t d = 0;
BOOST_FOREACH(const Index& idx, indices)
d += dim(idx);
// copy out values
Vector result(d);
size_t curHead = 0;
BOOST_FOREACH(const Index& j, indices) {
const SubVector& vj = at(j);
size_t dj = (size_t) vj.rows();
result.segment(curHead, dj) = vj;
curHead += dj;
}
return result;
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
this->values_[j] += c.values_[j];
else
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
}
/* ************************************************************************* */

171
gtsam/linear/VectorValues.h
View File

@ -111,7 +111,7 @@ namespace gtsam {
VectorValues() {}
/** Named constructor to create a VectorValues of the same structure of the
* specifed one, but filled with zeros.
* specified one, but filled with zeros.
* @return
*/
static VectorValues Zero(const VectorValues& model);
@ -127,14 +127,11 @@ namespace gtsam {
/** Return the dimension of variable \c j. */
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(); }
/** Return the dimension of each vector in this container */
std::vector<size_t> dims() const;
/** Check whether a variable with index \c j exists. */
bool exists(Index j) const { return j < size() && (*this)[j].rows() > 0; }
bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
/** 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). */
Vector& at(Index j) { checkExists(j); return values_[j]; }
@ -149,8 +146,8 @@ namespace gtsam {
const Vector& 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
* throws an invalid_argument exception if the index \c j is already used.
* Causes reallocation, but can insert values in any order.
* Throws an invalid_argument exception if the index \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated.
*/
@ -167,7 +164,7 @@ namespace gtsam {
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ",
const IndexFormatter& formatter =DefaultIndexFormatter) const;
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol = 1e-9) const;
@ -179,10 +176,10 @@ namespace gtsam {
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); }
explicit VectorValues(const CONTAINER& dimensions) { this->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) { this->resize(nVars, varDim); }
/** Named constructor to create a VectorValues that matches the structure of
* the specified VectorValues, but do not initialize the new values. */
@ -214,17 +211,16 @@ namespace gtsam {
void resizeLike(const VectorValues& other);
/** Resize the VectorValues to hold \c nVars variables, each of dimension
* \c varDim, not preserving any data. After calling this function, all
* variables will be uninitialized.
* \c varDim. Any individual vectors that do not change size will keep
* their values, but any new or resized vectors 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, 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.
* in \c dimensions. Any individual vectors that do not change size will keep
* their values, but any new or resized vectors will be uninitialized.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
@ -243,14 +239,11 @@ namespace gtsam {
/** Set all entries to zero, does not modify the size. */
void setZero();
/** Reference the entire solution vector (const version). */
//const Vector& asVector() const { return values_; }
/** Reference the entire solution vector. */
//Vector& asVector() { return values_; }
/** Retrieve the entire solution as a single vector */
const Vector asVector() const;
/** Access a vector that is a subset of relevant indices */
Vector vector(const std::vector<Index>& indices) const;
const Vector vector(const std::vector<Index>& indices) const;
/** Check whether this VectorValues has the same structure, meaning has the
* same number of variables and that all variables are of the same dimension,
@ -263,7 +256,7 @@ namespace gtsam {
/**
* Permute the entries of this VectorValues in place
*/
void permute(const Permutation& permutation);
void permuteInPlace(const Permutation& permutation);
/**
* Swap the data in this VectorValues with another.
@ -276,11 +269,14 @@ namespace gtsam {
/** Dot product with another VectorValues, interpreting both as vectors of
* their concatenated values. */
double dot(const VectorValues& V) const;
double dot(const VectorValues& v) const;
/** Vector L2 norm */
double norm() const;
/** Squared vector L2 norm */
double squaredNorm() const;
/**
* + operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
@ -316,50 +312,54 @@ namespace gtsam {
/**
* scale a vector by a scalar
*/
friend VectorValues operator*(const double a, const VectorValues &V) {
VectorValues result(VectorValues::SameStructure(V));
result.values_ = a * V.values_;
friend VectorValues operator*(const double a, const VectorValues &v) {
VectorValues result = VectorValues::SameStructure(v);
for(Index j = 0; j < v.size(); ++j)
result.values_[j] = a * v.values_[j];
return result;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend size_t dim(const VectorValues& V) {
return V.dim();
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend double dot(const VectorValues& V1, const VectorValues& V2) {
return gtsam::dot(V1.values_, V2.values_);
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void scal(double alpha, VectorValues& x) {
gtsam::scal(alpha, x.values_);
for(Index j = 0; j < x.size(); ++j)
x.values_[j] *= alpha;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void axpy(double alpha, const VectorValues& x, VectorValues& y) {
gtsam::axpy(alpha, x.values_, y.values_);
if(x.size() != y.size())
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
y.values_[j] += alpha * x.values_[j];
else
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void sqrt(VectorValues &x) {
Vector y = gtsam::esqrt(x.values_);
x.values_ = y;
for(Index j = 0; j < x.size(); ++j)
x.values_[j] = x.values_[j].cwiseSqrt();
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void ediv(const VectorValues& numerator,
const VectorValues& denominator, VectorValues &result) {
assert(
numerator.dim() == denominator.dim() && denominator.dim() == result.dim());
const size_t sz = result.dim();
for (size_t i = 0; i < sz; ++i)
result.values_[i] = numerator.values_[i] / denominator.values_[i];
friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) {
if(numerator.size() != denominator.size() || numerator.size() != result.size())
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
for(Index j = 0; j < numerator.size(); ++j)
if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
else
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void edivInPlace(VectorValues& x, const VectorValues& y) {
assert(x.dim() == y.dim());
const size_t sz = x.dim();
for (size_t i = 0; i < sz; ++i)
x.values_[i] /= y.values_[i];
if(x.size() != y.size())
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
x.values_[j].array() /= y.values_[j].array();
else
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
}
private:
@ -383,8 +383,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONTAINER>
void VectorValues::append(const CONTAINER& dimensions) {
values_.resize(size() + dimensions.size());
size_t i = size();
values_.resize(size() + dimensions.size());
BOOST_FOREACH(size_t dim, dimensions) {
values_[i] = Vector(dim);
++ i;
@ -395,51 +395,56 @@ namespace gtsam {
template<class CONTAINER>
VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
VectorValues ret;
values_.resize(dimensions.size());
ret.values_.resize(dimensions.size());
size_t i = 0;
BOOST_FOREACH(size_t dim, dimensions) {
values_[i] = Vector::Zero(dim);
ret.values_[i] = Vector::Zero(dim);
++ i;
}
return ret;
}
namespace internal {
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<typename ITERATOR>
Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values[*j].rows();
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<typename ITERATOR>
const Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
// If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
if(!allowNonexistant || values.exists(*j))
dim += values.dim(*j);
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values[*j].rows()) = values[*j];
varStart += values[*j].rows();
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
// If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
if(!allowNonexistant || values.exists(*j)) {
ret.segment(varStart, values.dim(*j)) = values[*j];
varStart += values.dim(*j);
}
}
return ret;
}
return ret;
}
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
assert(varStart == vector.rows());
}
}
} // \namespace gtsam

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

@ -39,7 +39,6 @@ TEST(VectorValues, insert) {
// 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));
@ -59,7 +58,7 @@ TEST(VectorValues, insert) {
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_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@ -80,7 +79,6 @@ TEST(VectorValues, dimsConstructor) {
// 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));
@ -106,7 +104,6 @@ TEST(VectorValues, copyConstructor) {
// 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));
@ -149,7 +146,6 @@ TEST(VectorValues, assignment) {
// 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));
@ -188,7 +184,6 @@ TEST(VectorValues, SameStructure) {
// 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));
@ -220,7 +215,6 @@ TEST(VectorValues, Zero_fromModel) {
// 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));
@ -256,7 +250,6 @@ TEST(VectorValues, Zero_fromDims) {
// 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));
@ -273,7 +266,6 @@ TEST(VectorValues, Zero_fromUniform) {
// 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));
@ -298,7 +290,6 @@ TEST(VectorValues, resizeLike) {
// 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));
@ -328,7 +319,6 @@ TEST(VectorValues, resize_fromUniform) {
// 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));
@ -355,7 +345,6 @@ TEST(VectorValues, resize_fromDims) {
// 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));
@ -383,7 +372,6 @@ TEST(VectorValues, append) {
// Check dimensions
LONGS_EQUAL(5, actual.size());
LONGS_EQUAL(13, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -443,7 +431,8 @@ TEST(VectorValues, permute) {
permutation[2] = 3;
permutation[3] = 1;
VectorValues actual = original.permute(permutation);
VectorValues actual = original;
actual.permuteInPlace(permutation);
EXPECT(assert_equal(expected, actual));
}

10
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -28,13 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
// Get magnitude of each update and find out which segment Delta falls in
assert(Delta >= 0.0);
double DeltaSq = Delta*Delta;
double x_u_norm_sq = dx_u.asVector().squaredNorm();
double x_n_norm_sq = dx_n.asVector().squaredNorm();
double x_u_norm_sq = dx_u.squaredNorm();
double x_n_norm_sq = dx_n.squaredNorm();
if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl;
if(DeltaSq < x_u_norm_sq) {
// Trust region is smaller than steepest descent update
VectorValues x_d = VectorValues::SameStructure(dx_u);
x_d.asVector() = dx_u.asVector() * std::sqrt(DeltaSq / x_u_norm_sq);
VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u;
if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
return x_d;
} else if(DeltaSq < x_n_norm_sq) {
@ -79,8 +78,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues&
// Compute blended point
if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
VectorValues blend = VectorValues::SameStructure(x_u);
blend.asVector() = (1. - tau) * x_u.asVector() + tau * x_n.asVector();
VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend);
return blend;
}

4
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose);
gttoc(Dog_leg_point);
if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << std::endl;
if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
gttic(retract);
// Compute expmapped solution
@ -208,7 +208,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(rho >= 0.75) {
// M agrees very well with f, so try to increase lambda
const double dx_d_norm = result.dx_d.vector().norm();
const double dx_d_norm = result.dx_d.norm();
const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)

10
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -39,15 +39,15 @@ void ISAM2::Impl::AddVariables(
// Add the new keys onto the ordering, add zeros to the delta for the new variables
std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
const size_t originalDim = delta.dim();
const size_t originalnVars = delta.size();
delta.append(dims);
delta.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaNewton.append(dims);
deltaNewton.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
RgProd.append(dims);
RgProd.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
for(Index j = originalnVars; j < delta.size(); ++j) {
delta[j].setZero();
deltaNewton[j].setZero();
RgProd[j].setZero();
}
{
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {

4
gtsam/nonlinear/ISAM2-inl.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
template<class VALUE>
VALUE ISAM2::calculateEstimate(Key key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
const Vector& delta = getDelta()[index];
return theta_.at<VALUE>(key).retract(delta);
}
@ -158,7 +158,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
if(!valuesChanged) {
const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
const SubVector& newValue(delta[*it]);
const Vector& newValue(delta[*it]);
if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
valuesChanged = true;
break;

14
gtsam/nonlinear/ISAM2.cpp
View File

@ -348,9 +348,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
variableIndex_.permuteInPlace(*colamd);
gttoc(permute_global_variable_index);
gttic(permute_delta);
delta_ = delta_.permute(*colamd);
deltaNewton_ = deltaNewton_.permute(*colamd);
RgProd_ = RgProd_.permute(*colamd);
delta_.permuteInPlace(*colamd);
deltaNewton_.permuteInPlace(*colamd);
RgProd_.permuteInPlace(*colamd);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.permuteWithInverse(*colamdInverse);
@ -484,9 +484,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
gttoc(permute_global_variable_index);
gttic(permute_delta);
delta_ = delta_.permute(partialSolveResult.fullReordering);
deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
delta_.permuteInPlace(partialSolveResult.fullReordering);
deltaNewton_.permuteInPlace(partialSolveResult.fullReordering);
RgProd_.permuteInPlace(partialSolveResult.fullReordering);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
@ -911,7 +911,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
gttic(Compute_point);
// Compute steepest descent point
grad.asVector() *= step;
scal(step, grad);
gttoc(Compute_point);
}

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -110,7 +110,7 @@ void LevenbergMarquardtOptimizer::iterate() {
// Solve Damped Gaussian Factor Graph
const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.asVector().norm() << endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
// update values

5
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -198,6 +198,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
// Determine delta between linearization points using new ordering
VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering);
Vector deltaVector = delta.asVector();
// clone and reorder linear factor to final ordering
GaussianFactor::shared_ptr linFactor = order(localOrdering);
@ -208,8 +209,8 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
HessianFactor::shared_ptr hesFactor = boost::shared_dynamic_cast<HessianFactor>(linFactor);
size_t dim = hesFactor->linearTerm().size();
Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * delta.vector();
hesFactor->constantTerm() += delta.vector().dot(G_delta) + delta.vector().dot(hesFactor->linearTerm());
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
hesFactor->constantTerm() += deltaVector.dot(G_delta) + deltaVector.dot(hesFactor->linearTerm());
hesFactor->linearTerm() += G_delta;
}

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -44,7 +44,7 @@ NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradient
}
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
Gradient step = g;
step.asVector() *= alpha;
scal(alpha, step);
return current.retract(step, ordering_);
}

2
gtsam/nonlinear/Values.cpp
View File

@ -81,7 +81,7 @@ namespace gtsam {
Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values

19
tests/testDoglegOptimizer.cpp
View File

@ -32,6 +32,7 @@
#pragma GCC diagnostic pop
#include <boost/assign/list_of.hpp> // for 'list_of()'
#include <functional>
#include <boost/iterator/counting_iterator.hpp>
using namespace std;
using namespace gtsam;
@ -45,7 +46,8 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
// Convert Vector to VectorValues
VectorValues vv = *allocateVectorValues(gbn);
vv.asVector() = values;
internal::writeVectorValuesSlices(values, vv,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
// Convert to factor graph
GaussianFactorGraph gfg(gbn);
@ -57,7 +59,8 @@ double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector
// Convert Vector to VectorValues
VectorValues vv = *allocateVectorValues(gbt);
vv.asVector() = values;
internal::writeVectorValuesSlices(values, vv,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
// Convert to factor graph
GaussianFactorGraph gfg(gbt);
@ -96,13 +99,15 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(VectorValues::Zero(gradientValues).asVector()));
gradientValues.asVector() = gradient;
internal::writeVectorValuesSlices(gradient, gradientValues,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10);
internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
// Compute the steepest descent point
@ -276,13 +281,15 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
LieVector(VectorValues::Zero(gradientValues).asVector()));
gradientValues.asVector() = gradient;
internal::writeVectorValuesSlices(gradient, gradientValues,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValues denseMatrixGradient = *allocateVectorValues(bt);
denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10);
internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
// Compute the steepest descent point

1
tests/testGaussianISAM2.cpp
View File

@ -207,6 +207,7 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) {
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
EXPECT(assert_equal(orderingExpected, ordering));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, ImplRemoveVariables) {