52 lines
1.4 KiB
C++
52 lines
1.4 KiB
C++
/**
|
|
* @file Errors.cpp
|
|
* @brief Factor Graph Configuration
|
|
* @brief Errors
|
|
* @author Carlos Nieto
|
|
* @author Christian Potthast
|
|
*/
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include "Errors.h"
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
void Errors::print(const std::string& s) const {
|
|
odprintf("%s:\n", s.c_str());
|
|
BOOST_FOREACH(const Vector& v, *this)
|
|
gtsam::print(v);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
struct equalsVector : public std::binary_function<const Vector&, const Vector&, bool> {
|
|
double tol_;
|
|
equalsVector(double tol = 1e-9) : tol_(tol) {}
|
|
bool operator()(const Vector& expected, const Vector& actual) {
|
|
return equal_with_abs_tol(expected, actual,tol_);
|
|
}
|
|
};
|
|
|
|
bool Errors::equals(const Errors& expected, double tol) const {
|
|
if( size() != expected.size() ) return false;
|
|
return equal(begin(),end(),expected.begin(),equalsVector(tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
double dot(const Errors& a, const Errors& b) {
|
|
size_t m = a.size();
|
|
if (b.size()!=m)
|
|
throw(std::invalid_argument("Errors::dot: incompatible sizes"));
|
|
double result = 0.0;
|
|
Errors::const_iterator it = b.begin();
|
|
BOOST_FOREACH(const Vector& ai, a)
|
|
result += gtsam::dot(ai, *(it++));
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // gtsam
|