Define Errors as a typedef to FastList<Vector>.
parent
36d6b70667
commit
3b369e5d8e
|
@ -26,16 +26,18 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors::Errors(const VectorValues& V) {
|
||||
for(const Vector& e: V | boost::adaptors::map_values) {
|
||||
push_back(e);
|
||||
Errors createErrors(const VectorValues& V) {
|
||||
Errors result;
|
||||
for (const Vector& e : V | boost::adaptors::map_values) {
|
||||
result.push_back(e);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Errors::print(const std::string& s) const {
|
||||
void print(const Errors& e, const string& s) {
|
||||
cout << s << endl;
|
||||
for(const Vector& v: *this)
|
||||
for(const Vector& v: e)
|
||||
gtsam::print(v);
|
||||
}
|
||||
|
||||
|
@ -48,50 +50,49 @@ struct equalsVector : public std::function<bool(const Vector&, const Vector&)> {
|
|||
}
|
||||
};
|
||||
|
||||
bool Errors::equals(const Errors& expected, double tol) const {
|
||||
if( size() != expected.size() ) return false;
|
||||
return equal(begin(),end(),expected.begin(),equalsVector(tol));
|
||||
bool equality(const Errors& actual, const Errors& expected, double tol) {
|
||||
if (actual.size() != expected.size()) return false;
|
||||
return equal(actual.begin(), actual.end(), expected.begin(),
|
||||
equalsVector(tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors Errors::operator+(const Errors& b) const {
|
||||
Errors operator+(const Errors& a, const Errors& b) {
|
||||
#ifndef NDEBUG
|
||||
size_t m = size();
|
||||
size_t m = a.size();
|
||||
if (b.size()!=m)
|
||||
throw(std::invalid_argument("Errors::operator+: incompatible sizes"));
|
||||
#endif
|
||||
Errors result;
|
||||
Errors::const_iterator it = b.begin();
|
||||
for(const Vector& ai: *this)
|
||||
for(const Vector& ai: a)
|
||||
result.push_back(ai + *(it++));
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors Errors::operator-(const Errors& b) const {
|
||||
Errors operator-(const Errors& a, const Errors& b) {
|
||||
#ifndef NDEBUG
|
||||
size_t m = size();
|
||||
size_t m = a.size();
|
||||
if (b.size()!=m)
|
||||
throw(std::invalid_argument("Errors::operator-: incompatible sizes"));
|
||||
#endif
|
||||
Errors result;
|
||||
Errors::const_iterator it = b.begin();
|
||||
for(const Vector& ai: *this)
|
||||
for(const Vector& ai: a)
|
||||
result.push_back(ai - *(it++));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors Errors::operator-() const {
|
||||
Errors operator-(const Errors& a) {
|
||||
Errors result;
|
||||
for(const Vector& ai: *this)
|
||||
for(const Vector& ai: a)
|
||||
result.push_back(-ai);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
double dot(const Errors& a, const Errors& b) {
|
||||
#ifndef NDEBUG
|
||||
|
@ -102,7 +103,7 @@ double dot(const Errors& a, const Errors& b) {
|
|||
double result = 0.0;
|
||||
Errors::const_iterator it = b.begin();
|
||||
for(const Vector& ai: a)
|
||||
result += gtsam::dot(ai, *(it++));
|
||||
result += gtsam::dot<Vector,Vector>(ai, *(it++));
|
||||
return result;
|
||||
}
|
||||
|
||||
|
@ -113,11 +114,6 @@ void axpy(double alpha, const Errors& x, Errors& y) {
|
|||
yi += alpha * (*(it++));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void print(const Errors& a, const string& s) {
|
||||
a.print(s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // gtsam
|
||||
|
|
|
@ -20,64 +20,54 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class VectorValues;
|
||||
// Forward declarations
|
||||
class VectorValues;
|
||||
|
||||
/** vector of errors */
|
||||
class GTSAM_EXPORT Errors : public FastList<Vector> {
|
||||
/// Errors is a vector of errors.
|
||||
using Errors = FastList<Vector>;
|
||||
|
||||
using Base = FastList<Vector>;
|
||||
/// Break V into pieces according to its start indices.
|
||||
GTSAM_EXPORT Errors createErrors(const VectorValues& V);
|
||||
|
||||
public:
|
||||
/// Print an Errors instance.
|
||||
GTSAM_EXPORT void print(const Errors& e, const std::string& s = "Errors");
|
||||
|
||||
using Base::Base; // inherit constructors
|
||||
// Check equality for unit testing.
|
||||
GTSAM_EXPORT bool equality(const Errors& actual, const Errors& expected,
|
||||
double tol = 1e-9);
|
||||
|
||||
/** Default constructor */
|
||||
Errors() = default;
|
||||
/// Addition.
|
||||
GTSAM_EXPORT Errors operator+(const Errors& a, const Errors& b);
|
||||
|
||||
/** break V into pieces according to its start indices */
|
||||
Errors(const VectorValues&V);
|
||||
/// Subtraction.
|
||||
GTSAM_EXPORT Errors operator-(const Errors& a, const Errors& b);
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "Errors") const;
|
||||
/// Negation.
|
||||
GTSAM_EXPORT Errors operator-(const Errors& a);
|
||||
|
||||
/** equals, for unit testing */
|
||||
bool equals(const Errors& expected, double tol=1e-9) const;
|
||||
/// Dot product.
|
||||
GTSAM_EXPORT double dot(const Errors& a, const Errors& b);
|
||||
|
||||
/** Addition */
|
||||
Errors operator+(const Errors& b) const;
|
||||
/// BLAS level 2 style AXPY, `y := alpha*x + y`
|
||||
GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
|
||||
|
||||
/** subtraction */
|
||||
Errors operator-(const Errors& b) const;
|
||||
/// traits
|
||||
template <>
|
||||
struct traits<Errors> {
|
||||
static void Print(const Errors& e, const std::string& str = "") {
|
||||
print(e, str);
|
||||
}
|
||||
static bool Equals(const Errors& actual, const Errors& expected,
|
||||
double tol = 1e-8) {
|
||||
return equality(actual, expected, tol);
|
||||
}
|
||||
};
|
||||
|
||||
/** negation */
|
||||
Errors operator-() const ;
|
||||
|
||||
}; // Errors
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
GTSAM_EXPORT double dot(const Errors& a, const Errors& b);
|
||||
|
||||
/**
|
||||
* BLAS level 2 style
|
||||
*/
|
||||
GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
|
||||
|
||||
/** print with optional string */
|
||||
GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error");
|
||||
|
||||
/// traits
|
||||
template<>
|
||||
struct traits<Errors> : public Testable<Errors> {
|
||||
};
|
||||
|
||||
} //\ namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
@ -110,7 +110,7 @@ VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
double SubgraphPreconditioner::error(const VectorValues& y) const {
|
||||
Errors e(y);
|
||||
Errors e = createErrors(y);
|
||||
VectorValues x = this->x(y);
|
||||
Errors e2 = Ab2_.gaussianErrors(x);
|
||||
return 0.5 * (dot(e, e) + dot(e2,e2));
|
||||
|
@ -129,7 +129,7 @@ VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const {
|
|||
/* ************************************************************************* */
|
||||
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
|
||||
Errors SubgraphPreconditioner::operator*(const VectorValues &y) const {
|
||||
Errors e(y);
|
||||
Errors e = createErrors(y);
|
||||
VectorValues x = Rc1_.backSubstitute(y); /* x=inv(R1)*y */
|
||||
Errors e2 = Ab2_ * x; /* A2*x */
|
||||
e.splice(e.end(), e2);
|
||||
|
|
Loading…
Reference in New Issue