VV work
Richard Roberts
parent
06f836c0a7
commit
5b9271cb97
|
|
@ -607,7 +607,7 @@ break;
|
|||
|
||||
/* ************************************************************************* */
|
||||
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
|
||||
r.vector() = Vector::Zero(r.dim());
|
||||
r.asVector() = Vector::Zero(r.dim());
|
||||
Index i = 0;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
|
|
@ -621,7 +621,7 @@ break;
|
|||
|
||||
/* ************************************************************************* */
|
||||
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
|
||||
x.vector() = Vector::Zero(x.dim());
|
||||
x.asVector() = Vector::Zero(x.dim());
|
||||
Index i = 0;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
|
|
|
|||
|
|
@ -324,8 +324,8 @@ Matrix HessianFactor::information() const {
|
|||
double HessianFactor::error(const VectorValues& c) const {
|
||||
// error 0.5*(f - 2*x'*g + x'*G*x)
|
||||
const double f = constantTerm();
|
||||
const double xtg = c.vector().dot(linearTerm());
|
||||
const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.vector();
|
||||
const double xtg = c.asVector().dot(linearTerm());
|
||||
const double xGx = c.asVector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.asVector();
|
||||
|
||||
return 0.5 * (f - 2.0 * xtg + xGx);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,30 +24,18 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues::VectorValues(const VectorValues& other) {
|
||||
*this = other;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues& VectorValues::operator=(const VectorValues& rhs) {
|
||||
if(this != &rhs) {
|
||||
resizeLike(rhs); // Copy structure
|
||||
values_ = rhs.values_; // Copy values
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues VectorValues::Zero(const VectorValues& x) {
|
||||
VectorValues cloned(SameStructure(x));
|
||||
cloned.setZero();
|
||||
return cloned;
|
||||
VectorValues result;
|
||||
result.values_.resize(x.size());
|
||||
for(size_t j=0; j<x.size(); ++j)
|
||||
result.values_[j] = Vector::Zero(x.dim(j));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
vector<size_t> VectorValues::dims() const {
|
||||
std::vector<size_t> result(this->size());
|
||||
vector<size_t> result(this->size());
|
||||
for(Index j = 0; j < this->size(); ++j)
|
||||
result[j] = this->dim(j);
|
||||
return result;
|
||||
|
|
@ -59,44 +47,30 @@ void VectorValues::insert(Index j, const Vector& value) {
|
|||
if(exists(j))
|
||||
throw invalid_argument("VectorValues: requested variable index to insert already exists.");
|
||||
|
||||
// Get vector of dimensions
|
||||
FastVector<size_t> dimensions(size());
|
||||
for(size_t k=0; k<maps_.size(); ++k)
|
||||
dimensions[k] = maps_[k].rows();
|
||||
|
||||
// If this adds variables at the end, insert zero-length entries up to j
|
||||
if(j >= size())
|
||||
dimensions.insert(dimensions.end(), j+1-size(), 0);
|
||||
values_.resize(j+1);
|
||||
|
||||
// Set correct dimension for j
|
||||
dimensions[j] = value.rows();
|
||||
|
||||
// Make a copy to make assignment easier
|
||||
VectorValues original(*this);
|
||||
|
||||
// Resize to accomodate new variable
|
||||
resize(dimensions);
|
||||
|
||||
// Copy original variables
|
||||
for(Index k = 0; k < original.size(); ++k)
|
||||
if(k != j && exists(k))
|
||||
operator[](k) = original[k];
|
||||
|
||||
// Copy new variable
|
||||
operator[](j) = value;
|
||||
// Assign value
|
||||
values_[j] = value;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const {
|
||||
std::cout << str << ": " << size() << " elements\n";
|
||||
for (Index var = 0; var < size(); ++var)
|
||||
std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n";
|
||||
std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n";
|
||||
std::cout.flush();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool VectorValues::equals(const VectorValues& x, double tol) const {
|
||||
return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol);
|
||||
if(this->size() != x.size())
|
||||
return false;
|
||||
for(size_t j=0; j<size(); ++j)
|
||||
if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -153,28 +127,6 @@ bool VectorValues::hasSameStructure(const VectorValues& other) const {
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues VectorValues::operator+(const VectorValues& c) const {
|
||||
assert(this->hasSameStructure(c));
|
||||
VectorValues result(SameStructure(c));
|
||||
result.values_ = this->values_ + c.values_;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues VectorValues::operator-(const VectorValues& c) const {
|
||||
assert(this->hasSameStructure(c));
|
||||
VectorValues result(SameStructure(c));
|
||||
result.values_ = this->values_ - c.values_;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VectorValues::operator+=(const VectorValues& c) {
|
||||
assert(this->hasSameStructure(c));
|
||||
this->values_ += c.values_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues VectorValues::permute(const Permutation& permutation) const {
|
||||
// Create result and allocate space
|
||||
|
|
@ -202,4 +154,36 @@ void VectorValues::swap(VectorValues& other) {
|
|||
this->maps_.swap(other.maps_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double VectorValues::dot(const VectorValues& V) const {
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double VectorValues::norm() const {
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues VectorValues::operator+(const VectorValues& c) const {
|
||||
assert(this->hasSameStructure(c));
|
||||
VectorValues result(SameStructure(c));
|
||||
result.values_ = this->values_ + c.values_;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues VectorValues::operator-(const VectorValues& c) const {
|
||||
assert(this->hasSameStructure(c));
|
||||
VectorValues result(SameStructure(c));
|
||||
result.values_ = this->values_ - c.values_;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VectorValues::operator+=(const VectorValues& c) {
|
||||
assert(this->hasSameStructure(c));
|
||||
this->values_ += c.values_;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -92,15 +92,14 @@ namespace gtsam {
|
|||
*/
|
||||
class VectorValues {
|
||||
protected:
|
||||
Vector values_; ///< The underlying vector storing the values
|
||||
typedef std::vector<SubVector> ValueMaps; ///< Collection of SubVector s
|
||||
ValueMaps maps_; ///< SubVector s referencing each vector variable in values_
|
||||
typedef std::vector<Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
|
||||
Values values_; ///< Collection of Vectors making up this VectorValues
|
||||
|
||||
public:
|
||||
typedef ValueMaps::iterator iterator; ///< Iterator over vector values
|
||||
typedef ValueMaps::const_iterator const_iterator; ///< Const iterator over vector values
|
||||
typedef ValueMaps::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
|
||||
typedef ValueMaps::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
|
||||
typedef Values::iterator iterator; ///< Iterator over vector values
|
||||
typedef Values::const_iterator const_iterator; ///< Const iterator over vector values
|
||||
typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
|
||||
typedef Values::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
|
||||
|
|
@ -111,9 +110,6 @@ namespace gtsam {
|
|||
*/
|
||||
VectorValues() {}
|
||||
|
||||
/** Copy constructor */
|
||||
VectorValues(const VectorValues &other);
|
||||
|
||||
/** Named constructor to create a VectorValues of the same structure of the
|
||||
* specifed one, but filled with zeros.
|
||||
* @return
|
||||
|
|
@ -126,31 +122,31 @@ namespace gtsam {
|
|||
|
||||
/** 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 values_.size(); }
|
||||
|
||||
/** 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(); }
|
||||
//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() && maps_[j].rows() > 0; }
|
||||
bool exists(Index j) const { return j < size() && (*this)[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). */
|
||||
SubVector& at(Index j) { checkExists(j); return maps_[j]; }
|
||||
Vector& at(Index j) { checkExists(j); return values_[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]; }
|
||||
const Vector& at(Index j) const { checkExists(j); return values_[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); }
|
||||
Vector& 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); }
|
||||
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
|
||||
|
|
@ -160,17 +156,14 @@ namespace gtsam {
|
|||
*/
|
||||
void insert(Index j, const Vector& value);
|
||||
|
||||
/** Assignment */
|
||||
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(); } ///< 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
|
||||
iterator begin() { return values_.begin(); } ///< Iterator over variables
|
||||
const_iterator begin() const { return values_.begin(); } ///< Iterator over variables
|
||||
iterator end() { return values_.end(); } ///< Iterator over variables
|
||||
const_iterator end() const { return values_.end(); } ///< Iterator over variables
|
||||
reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables
|
||||
const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables
|
||||
reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables
|
||||
const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables
|
||||
|
||||
/** print required by Testable for unit testing */
|
||||
void print(const std::string& str = "VectorValues: ",
|
||||
|
|
@ -251,10 +244,10 @@ namespace gtsam {
|
|||
void setZero();
|
||||
|
||||
/** Reference the entire solution vector (const version). */
|
||||
const Vector& vector() const { chk(); return values_; }
|
||||
//const Vector& asVector() const { return values_; }
|
||||
|
||||
/** Reference the entire solution vector. */
|
||||
Vector& vector() { chk(); return values_; }
|
||||
//Vector& asVector() { 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,
|
||||
|
|
@ -264,16 +257,26 @@ namespace gtsam {
|
|||
*/
|
||||
bool hasSameStructure(const VectorValues& other) const;
|
||||
|
||||
/**
|
||||
* Permute the entries of this VectorValues in place
|
||||
*/
|
||||
void permute(const Permutation& permutation);
|
||||
|
||||
/**
|
||||
* Swap the data in this VectorValues with another.
|
||||
*/
|
||||
void swap(VectorValues& other);
|
||||
|
||||
/// @}
|
||||
/// @name Linear algebra operations
|
||||
/// @{
|
||||
|
||||
/** Dot product with another VectorValues, interpreting both as vectors of
|
||||
* their concatenated values. */
|
||||
double dot(const VectorValues& V) const {
|
||||
return gtsam::dot(this->values_, V.values_);
|
||||
}
|
||||
double dot(const VectorValues& V) const;
|
||||
|
||||
/** Vector L2 norm */
|
||||
inline double norm() const {
|
||||
return this->vector().norm();
|
||||
}
|
||||
double norm() const;
|
||||
|
||||
/**
|
||||
* + operator does element-wise addition. Both VectorValues must have the
|
||||
|
|
@ -293,26 +296,11 @@ namespace gtsam {
|
|||
*/
|
||||
void operator+=(const VectorValues& c);
|
||||
|
||||
/**
|
||||
* Permute the entries of this VectorValues, returns a new VectorValues as
|
||||
* the result.
|
||||
*/
|
||||
VectorValues permute(const Permutation& permutation) const;
|
||||
|
||||
/**
|
||||
* Swap the data in this VectorValues with another.
|
||||
*/
|
||||
void swap(VectorValues& other);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
// Verifies that the underlying Vector is consistent with the collection of SubVectors
|
||||
void chk() const;
|
||||
|
||||
// Throw an exception if j does not exist
|
||||
void checkExists(Index j) const {
|
||||
chk();
|
||||
if(!exists(j))
|
||||
throw std::out_of_range("VectorValues: requested variable index is not in this VectorValues.");
|
||||
}
|
||||
|
|
@ -375,23 +363,9 @@ namespace gtsam {
|
|||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void save(ARCHIVE & ar, const unsigned int version) const {
|
||||
// The maps_ stores pointers, so we serialize dimensions instead
|
||||
std::vector<size_t> dimensions(size());
|
||||
for(size_t j=0; j<maps_.size(); ++j)
|
||||
dimensions[j] = maps_[j].rows();
|
||||
ar & BOOST_SERIALIZATION_NVP(dimensions);
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(values_);
|
||||
}
|
||||
template<class ARCHIVE>
|
||||
void load(ARCHIVE & ar, const unsigned int version) {
|
||||
std::vector<size_t> dimensions;
|
||||
ar & BOOST_SERIALIZATION_NVP(dimensions); // Load dimensions
|
||||
resize(dimensions); // Allocate space for everything
|
||||
ar & BOOST_SERIALIZATION_NVP(values_); // Load values
|
||||
chk();
|
||||
}
|
||||
BOOST_SERIALIZATION_SPLIT_MEMBER()
|
||||
}; // VectorValues definition
|
||||
|
||||
// Implementations of template and inline functions
|
||||
|
|
@ -399,57 +373,41 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class CONTAINER>
|
||||
void VectorValues::resize(const CONTAINER& dimensions) {
|
||||
maps_.clear();
|
||||
values_.resize(0);
|
||||
values_.clear();
|
||||
append(dimensions);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONTAINER>
|
||||
void VectorValues::append(const CONTAINER& dimensions) {
|
||||
chk();
|
||||
int newDim = std::accumulate(dimensions.begin(), dimensions.end(), 0); // Sum of dimensions
|
||||
values_.conservativeResize(dim() + newDim);
|
||||
// Relocate existing maps
|
||||
int varStart = 0;
|
||||
for(size_t j = 0; j < maps_.size(); ++j) {
|
||||
new (&maps_[j]) SubVector(values_.segment(varStart, maps_[j].rows()));
|
||||
varStart += maps_[j].rows();
|
||||
}
|
||||
maps_.reserve(maps_.size() + dimensions.size());
|
||||
values_.resize(size() + dimensions.size());
|
||||
size_t i = size();
|
||||
BOOST_FOREACH(size_t dim, dimensions) {
|
||||
maps_.push_back(values_.segment(varStart, dim));
|
||||
varStart += (int)dim; // varStart is continued from first for loop
|
||||
values_[i] = Vector(dim);
|
||||
++ i;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONTAINER>
|
||||
VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
|
||||
VectorValues ret(dimensions);
|
||||
ret.setZero();
|
||||
VectorValues ret;
|
||||
values_.resize(dimensions.size());
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(size_t dim, dimensions) {
|
||||
values_[i] = Vector::Zero(dim);
|
||||
++ i;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
inline void VectorValues::chk() const {
|
||||
#ifndef NDEBUG
|
||||
// Check that the first SubVector points to the beginning of the Vector
|
||||
if(maps_.size() > 0) {
|
||||
assert(values_.data() == maps_[0].data());
|
||||
// Check that the end of the last SubVector points to the end of the Vector
|
||||
assert(values_.rows() == maps_.back().data() + maps_.back().rows() - maps_.front().data());
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
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<class VALUES, typename ITERATOR>
|
||||
Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
|
||||
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)
|
||||
|
|
@ -469,8 +427,8 @@ namespace gtsam {
|
|||
// 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, class VALUES, typename ITERATOR>
|
||||
void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
|
||||
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) {
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ TEST(VectorValues, dimsConstructor) {
|
|||
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()));
|
||||
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -126,7 +126,7 @@ TEST(VectorValues, copyConstructor) {
|
|||
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);
|
||||
|
|
@ -169,7 +169,7 @@ TEST(VectorValues, assignment) {
|
|||
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);
|
||||
|
|
@ -337,7 +337,7 @@ TEST(VectorValues, resize_fromUniform) {
|
|||
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()));
|
||||
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -364,7 +364,7 @@ TEST(VectorValues, resize_fromDims) {
|
|||
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()));
|
||||
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -28,13 +28,13 @@ 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.vector().squaredNorm();
|
||||
double x_n_norm_sq = dx_n.vector().squaredNorm();
|
||||
double x_u_norm_sq = dx_u.asVector().squaredNorm();
|
||||
double x_n_norm_sq = dx_n.asVector().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.vector() = dx_u.vector() * std::sqrt(DeltaSq / x_u_norm_sq);
|
||||
x_d.asVector() = dx_u.asVector() * std::sqrt(DeltaSq / x_u_norm_sq);
|
||||
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) {
|
||||
|
|
@ -80,7 +80,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.vector() = (1. - tau) * x_u.vector() + tau * x_n.vector();
|
||||
blend.asVector() = (1. - tau) * x_u.asVector() + tau * x_n.asVector();
|
||||
return blend;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -43,11 +43,11 @@ void ISAM2::Impl::AddVariables(
|
|||
const size_t originalDim = delta.dim();
|
||||
const size_t originalnVars = delta.size();
|
||||
delta.append(dims);
|
||||
delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
delta.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaNewton.append(dims);
|
||||
deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaNewton.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
RgProd.append(dims);
|
||||
RgProd.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
RgProd.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
{
|
||||
Index nextVar = originalnVars;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
|
||||
|
|
|
|||
|
|
@ -907,13 +907,13 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
|
|||
|
||||
gttic(Compute_minimizing_step_size);
|
||||
// Compute minimizing step size
|
||||
double RgNormSq = isam.RgProd_.vector().squaredNorm();
|
||||
double RgNormSq = isam.RgProd_.asVector().squaredNorm();
|
||||
double step = -gradientSqNorm / RgNormSq;
|
||||
gttoc(Compute_minimizing_step_size);
|
||||
|
||||
gttic(Compute_point);
|
||||
// Compute steepest descent point
|
||||
grad.vector() *= step;
|
||||
grad.asVector() *= step;
|
||||
gttoc(Compute_point);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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.vector().norm() << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.asVector().norm() << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
|
||||
|
||||
// update values
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradient
|
|||
}
|
||||
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const {
|
||||
Gradient step = g;
|
||||
step.vector() *= alpha;
|
||||
step.asVector() *= alpha;
|
||||
return current.retract(step, ordering_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
|
|||
|
||||
// Convert Vector to VectorValues
|
||||
VectorValues vv = *allocateVectorValues(gbn);
|
||||
vv.vector() = values;
|
||||
vv.asVector() = values;
|
||||
|
||||
// Convert to factor graph
|
||||
GaussianFactorGraph gfg(gbn);
|
||||
|
|
@ -57,7 +57,7 @@ double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector
|
|||
|
||||
// Convert Vector to VectorValues
|
||||
VectorValues vv = *allocateVectorValues(gbt);
|
||||
vv.vector() = values;
|
||||
vv.asVector() = values;
|
||||
|
||||
// Convert to factor graph
|
||||
GaussianFactorGraph gfg(gbt);
|
||||
|
|
@ -89,20 +89,20 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
|
|||
// Compute the Hessian numerically
|
||||
Matrix hessian = numericalHessian(
|
||||
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
|
||||
LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).vector()));
|
||||
LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector()));
|
||||
|
||||
// Compute the gradient numerically
|
||||
VectorValues gradientValues = *allocateVectorValues(gbn);
|
||||
Vector gradient = numericalGradient(
|
||||
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
|
||||
LieVector(VectorValues::Zero(gradientValues).vector()));
|
||||
gradientValues.vector() = gradient;
|
||||
LieVector(VectorValues::Zero(gradientValues).asVector()));
|
||||
gradientValues.asVector() = gradient;
|
||||
|
||||
// Compute the gradient using dense matrices
|
||||
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
|
||||
LONGS_EQUAL(11, augmentedHessian.cols());
|
||||
VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
|
||||
denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
|
||||
denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10);
|
||||
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
|
||||
|
||||
// Compute the steepest descent point
|
||||
|
|
@ -269,20 +269,20 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
|
|||
// Compute the Hessian numerically
|
||||
Matrix hessian = numericalHessian(
|
||||
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
|
||||
LieVector(VectorValues::Zero(*allocateVectorValues(bt)).vector()));
|
||||
LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector()));
|
||||
|
||||
// Compute the gradient numerically
|
||||
VectorValues gradientValues = *allocateVectorValues(bt);
|
||||
Vector gradient = numericalGradient(
|
||||
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
|
||||
LieVector(VectorValues::Zero(gradientValues).vector()));
|
||||
gradientValues.vector() = gradient;
|
||||
LieVector(VectorValues::Zero(gradientValues).asVector()));
|
||||
gradientValues.asVector() = gradient;
|
||||
|
||||
// Compute the gradient using dense matrices
|
||||
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
|
||||
LONGS_EQUAL(11, augmentedHessian.cols());
|
||||
VectorValues denseMatrixGradient = *allocateVectorValues(bt);
|
||||
denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
|
||||
denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10);
|
||||
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
|
||||
|
||||
// Compute the steepest descent point
|
||||
|
|
@ -338,12 +338,12 @@ TEST(DoglegOptimizer, ComputeBlend) {
|
|||
VectorValues xn = optimize(gbn);
|
||||
|
||||
// The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
|
||||
EXPECT(xu.vector().norm() < xn.vector().norm());
|
||||
EXPECT(xu.asVector().norm() < xn.asVector().norm());
|
||||
|
||||
// Compute blend
|
||||
double Delta = 1.5;
|
||||
VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
|
||||
DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
|
||||
DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -371,12 +371,12 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
|
|||
|
||||
double Delta1 = 0.5; // Less than steepest descent
|
||||
VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
|
||||
DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);
|
||||
DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5);
|
||||
|
||||
double Delta2 = 1.5; // Between steepest descent and Newton's method
|
||||
VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
|
||||
VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
|
||||
DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
|
||||
DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5);
|
||||
EXPECT(assert_equal(expected2, actual2));
|
||||
|
||||
double Delta3 = 5.0; // Larger than Newton's method point
|
||||
|
|
|
|||
Loading…
Reference in New Issue