54 lines
1.7 KiB
C++
54 lines
1.7 KiB
C++
/**
|
|
* @file NonlinearFactorGraph-inl.h
|
|
* @brief Factor Graph Consisting of non-linear factors
|
|
* @author Frank Dellaert
|
|
* @author Carlos Nieto
|
|
* @author Christian Potthast
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "GaussianFactorGraph.h"
|
|
#include "NonlinearFactorGraph.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
template<class Config>
|
|
double NonlinearFactorGraph<Config>::error(const Config& c) const {
|
|
double total_error = 0.;
|
|
// iterate over all the factors_ to accumulate the log probabilities
|
|
typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
|
|
const_iterator;
|
|
for (const_iterator factor = this->factors_.begin(); factor
|
|
!= this->factors_.end(); factor++)
|
|
total_error += (*factor)->error(c);
|
|
|
|
return total_error;
|
|
}
|
|
/* ************************************************************************* */
|
|
template<class Config>
|
|
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_(
|
|
const Config& config) const{
|
|
|
|
// create an empty linear FG
|
|
boost::shared_ptr<GaussianFactorGraph> linearFG(new GaussianFactorGraph);
|
|
|
|
typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
|
|
const_iterator;
|
|
// linearize all factors
|
|
for (const_iterator factor = this->factors_.begin(); factor
|
|
< this->factors_.end(); factor++) {
|
|
boost::shared_ptr<GaussianFactor> lf = (*factor)->linearize(config);
|
|
linearFG->push_back(lf);
|
|
}
|
|
return linearFG;
|
|
}
|
|
/* ************************************************************************* */
|
|
template<class Config>
|
|
GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
|
|
const Config& config) const {
|
|
return *linearize_(config);
|
|
}
|
|
}
|