40 lines
1.1 KiB
C
40 lines
1.1 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 "LinearFactorGraph.h"
|
||
|
|
#include "NonlinearFactorGraph.h"
|
||
|
|
|
||
|
|
namespace gtsam {
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
template<class Config>
|
||
|
|
LinearFactorGraph NonlinearFactorGraph<Config>::linearize(const Config& config) const {
|
||
|
|
// TODO speed up the function either by returning a pointer or by
|
||
|
|
// returning the linearisation as a second argument and returning
|
||
|
|
// the reference
|
||
|
|
|
||
|
|
// create an empty linear FG
|
||
|
|
LinearFactorGraph linearFG;
|
||
|
|
|
||
|
|
typedef typename FactorGraph<NonlinearFactor<Config> ,Config>:: const_iterator const_iterator;
|
||
|
|
// linearize all factors
|
||
|
|
for (const_iterator factor = this->factors_.begin(); factor
|
||
|
|
< this->factors_.end(); factor++) {
|
||
|
|
boost::shared_ptr<LinearFactor> lf = (*factor)->linearize(config);
|
||
|
|
linearFG.push_back(lf);
|
||
|
|
}
|
||
|
|
|
||
|
|
return linearFG;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
|
||
|
|
}
|