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

Made NonlinearFactor inherit from Factor<Symbol>, also hopefully fixed a compile error when implicitly up-casting from a derived factor type to its base type, which previously had some workaround(s) in Alex's code and in DiscreteFactor

release/4.3a0
Richard Roberts 2011-03-04 00:18:17 +00:00
parent d96b114618
commit e4ed8cfd60
9 changed files with 32 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/inference/Factor.h
View File

@ -60,16 +60,16 @@ public:
* Typedef to the conditional type obtained by eliminating this factor.
* Derived classes must redefine this.
*/
typedef gtsam::Conditional<Key> ConditionalType;
typedef Conditional<Key> ConditionalType;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<Factor> shared_ptr;
/** Iterator over keys */
typedef std::vector<Index>::iterator iterator;
typedef typename std::vector<Key>::iterator iterator;
/** Const iterator over keys */
typedef std::vector<Index>::const_iterator const_iterator;
typedef typename std::vector<Key>::const_iterator const_iterator;
protected:

4
gtsam/linear/GaussianConditional.cpp
View File

@ -142,8 +142,8 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianConditional::toFactor() const {
return GaussianFactor::shared_ptr(new JacobianFactor(*this));
JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
return JacobianFactor::shared_ptr(new JacobianFactor(*this));
}
/* ************************************************************************* */

3
gtsam/linear/GaussianConditional.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/types.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h>
@ -122,7 +123,7 @@ public:
* Copy to a Factor (this creates a JacobianFactor and returns it as its
* base class GaussianFactor.
*/
boost::shared_ptr<GaussianFactor> toFactor() const;
boost::shared_ptr<JacobianFactor> toFactor() const;
/**
* solve a conditional Gaussian

3
gtsam/linear/GaussianFactor.cpp
View File

@ -32,6 +32,9 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianFactor::GaussianFactor(const GaussianConditional& c) : IndexFactor(c) {}
/* ************************************************************************* */
pair<GaussianBayesNet::shared_ptr, GaussianFactor::shared_ptr> GaussianFactor::CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals, SolveMethod solveMethod) {

9
gtsam/linear/GaussianFactor.h
View File

@ -21,7 +21,7 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/linear/Errors.h>
#include <string>
@ -29,8 +29,11 @@
namespace gtsam {
// Forward declarations
class VectorValues;
class Permutation;
class GaussianConditional;
template<class C> class BayesNet;
/**
* Base Class for a linear factor.
@ -45,7 +48,7 @@ namespace gtsam {
GaussianFactor(const This& f) : IndexFactor(f) {}
/** Construct from derived type */
GaussianFactor(const GaussianConditional& c) : IndexFactor(c) {}
GaussianFactor(const GaussianConditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> GaussianFactor(KeyIterator beginKey, KeyIterator endKey) :
@ -96,7 +99,7 @@ namespace gtsam {
/**
* Combine and eliminate several factors.
*/
static std::pair<GaussianBayesNet::shared_ptr, shared_ptr> CombineAndEliminate(
static std::pair<boost::shared_ptr<BayesNet<GaussianConditional> >, shared_ptr> CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals=1, SolveMethod solveMethod=SOLVE_QR);
}; // GaussianFactor

2
gtsam/linear/GaussianISAM.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/inference/ISAM.h>

7
gtsam/linear/HessianFactor.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/linear/GaussianFactor.h>
@ -29,6 +30,8 @@ namespace gtsam {
// Forward declarations
class JacobianFactor;
class SharedDiagonal;
class GaussianConditional;
template<class C> class BayesNet;
// Definition of Scatter
struct SlotEntry {
@ -47,7 +50,7 @@ namespace gtsam {
BlockInfo info_; // The block view of the full information matrix.
void assertInvariants() const;
GaussianBayesNet::shared_ptr splitEliminatedFactor(size_t nrFrontals, const std::vector<Index>& keys);
boost::shared_ptr<BayesNet<GaussianConditional> > splitEliminatedFactor(size_t nrFrontals, const std::vector<Index>& keys);
void updateATA(const JacobianFactor& update, const Scatter& scatter);
void updateATA(const HessianFactor& update, const Scatter& scatter);
@ -123,7 +126,7 @@ namespace gtsam {
/**
* Combine and eliminate several factors.
*/
static std::pair<GaussianBayesNet::shared_ptr, shared_ptr> CombineAndEliminate(
static std::pair<boost::shared_ptr<BayesNet<GaussianConditional> >, shared_ptr> CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals=1);
// Friend unit test classes

9
gtsam/linear/JacobianFactor.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/types.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/Errors.h>
@ -36,6 +37,8 @@ namespace gtsam {
// Forward declarations
class HessianFactor;
class VariableSlots;
class GaussianConditional;
template<class C> class BayesNet;
class JacobianFactor : public GaussianFactor {
@ -194,12 +197,12 @@ namespace gtsam {
/**
* Combine and eliminate several factors.
*/
static std::pair<GaussianBayesNet::shared_ptr, shared_ptr> CombineAndEliminate(
static std::pair<boost::shared_ptr<BayesNet<GaussianConditional> >, shared_ptr> CombineAndEliminate(
const FactorGraph<JacobianFactor>& factors, size_t nrFrontals=1);
GaussianConditional::shared_ptr eliminateFirst();
boost::shared_ptr<GaussianConditional> eliminateFirst();
GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1);
boost::shared_ptr<BayesNet<GaussianConditional> > eliminate(size_t nrFrontals = 1);
// Friend HessianFactor to facilitate convertion constructors
friend class HessianFactor;

22
gtsam/nonlinear/NonlinearFactor.h
View File

@ -26,6 +26,7 @@
#include <boost/shared_ptr.hpp>
#include <boost/serialization/base_object.hpp>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
@ -54,14 +55,13 @@ namespace gtsam {
* which are objects in non-linear manifolds (Lie groups).
*/
template<class VALUES>
class NonlinearFactor: public Testable<NonlinearFactor<VALUES> > {
class NonlinearFactor: public Factor<Symbol>, public Testable<NonlinearFactor<VALUES> > {
protected:
typedef NonlinearFactor<VALUES> This;
SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */
public:
@ -100,32 +100,16 @@ namespace gtsam {
return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c));
}
/** return keys */
const std::list<Symbol>& keys() const {
return keys_;
}
/** get the dimension of the factor (number of rows on linearization) */
size_t dim() const {
return noiseModel_->dim();
}
/* return the begin iterator of keys */
std::list<Symbol>::const_iterator begin() const { return keys_.begin(); }
/* return the end iterator of keys */
std::list<Symbol>::const_iterator end() const { return keys_.end(); }
/** access to the noise model */
SharedGaussian get_noiseModel() const {
return noiseModel_;
}
/** get the size of the factor */
std::size_t size() const {
return keys_.size();
}
/** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const VALUES& c) const = 0;
@ -310,6 +294,7 @@ namespace gtsam {
NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1,
KEY2 j2) :
Base(noiseModel), key1_(j1), key2_(j2) {
this->keys_.reserve(2);
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
}
@ -450,6 +435,7 @@ namespace gtsam {
*/
NonlinearFactor3(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) :
Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) {
this->keys_.reserve(3);
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
this->keys_.push_back(key3_);