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