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

error_vectors

release/4.3a0
Frank Dellaert 2009-12-18 06:55:24 +00:00
parent bf504d30db
commit c987ab397c
2 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cpp/NonlinearFactorGraph-inl.h
View File

@ -12,8 +12,19 @@
#include "GaussianFactorGraph.h" #include "GaussianFactorGraph.h"
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
template<class Config>
Vector NonlinearFactorGraph<Config>::error_vector(const Config& c) const {
list<Vector> errors;
BOOST_FOREACH(typename NonlinearFactorGraph<Config>::sharedFactor factor, this->factors_)
errors.push_back(factor->error_vector(c));
return concatVectors(errors);
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Config>
double NonlinearFactorGraph<Config>::error(const Config& c) const { double NonlinearFactorGraph<Config>::error(const Config& c) const {
@ -21,9 +32,9 @@ namespace gtsam {
// iterate over all the factors_ to accumulate the log probabilities // iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(typename NonlinearFactorGraph<Config>::sharedFactor factor, this->factors_) BOOST_FOREACH(typename NonlinearFactorGraph<Config>::sharedFactor factor, this->factors_)
total_error += factor->error(c); total_error += factor->error(c);
return total_error; return total_error;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Config>
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_( boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_(

12
cpp/NonlinearFactorGraph.h
View File

@ -31,6 +31,9 @@ namespace gtsam {
/** unnormalized error */ /** unnormalized error */
double error(const Config& c) const; double error(const Config& c) const;
/** all individual errors */
Vector error_vector(const Config& c) const;
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const Config& c) const { double probPrime(const Config& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
@ -41,10 +44,11 @@ namespace gtsam {
*/ */
GaussianFactorGraph linearize(const Config& config) const; GaussianFactorGraph linearize(const Config& config) const;
/** /**
* shared pointer versions for MATLAB * shared pointer versions for MATLAB
*/ */
boost::shared_ptr<GaussianFactorGraph> linearize_(const Config& config) const; boost::shared_ptr<GaussianFactorGraph>
linearize_(const Config& config) const;
}; };