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

linearize method for Pose2Graph works in Matlab!!

release/4.3a0
Viorela Ila 2009-12-11 21:34:08 +00:00
parent 03304ac421
commit 76b55aba7d
6 changed files with 24 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
cpp/NonlinearFactorGraph-inl.h
View File

@ -28,27 +28,26 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Config>
GaussianFactorGraph NonlinearFactorGraph<Config>::linearize( boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_(
const Config& config) const { 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 // create an empty linear FG
GaussianFactorGraph linearFG; boost::shared_ptr<GaussianFactorGraph> linearFG(new GaussianFactorGraph);
typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
const_iterator; const_iterator;
// linearize all factors // linearize all factors
for (const_iterator factor = this->factors_.begin(); factor for (const_iterator factor = this->factors_.begin(); factor
< this->factors_.end(); factor++) { < this->factors_.end(); factor++) {
boost::shared_ptr<GaussianFactor> lf = (*factor)->linearize(config); boost::shared_ptr<GaussianFactor> lf = (*factor)->linearize(config);
linearFG.push_back(lf); linearFG->push_back(lf);
} }
return linearFG; return linearFG;
} }
/* ************************************************************************* */
/* ************************************************************************* */ template<class Config>
GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
const Config& config) const {
return *linearize_(config);
}
} }

5
cpp/NonlinearFactorGraph.h
View File

@ -41,6 +41,11 @@ namespace gtsam {
*/ */
GaussianFactorGraph linearize(const Config& config) const; GaussianFactorGraph linearize(const Config& config) const;
/**
* shared pointer versions for MATLAB
*/
boost::shared_ptr<GaussianFactorGraph> linearize_(const Config& config) const;
}; };
} // namespace } // namespace

1
cpp/Ordering.h
View File

@ -53,6 +53,7 @@ namespace gtsam {
* @return bool * @return bool
*/ */
bool equals(const Ordering &ord, double tol=0) const; bool equals(const Ordering &ord, double tol=0) const;
}; };
} }

2
cpp/Pose2Graph.cpp
View File

@ -12,7 +12,7 @@ namespace gtsam {
// explicit instantiation so all the code is there and we can link with it // explicit instantiation so all the code is there and we can link with it
template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ; template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
template class NonlinearFactorGraph<Pose2Factor> ; template class NonlinearFactorGraph<Pose2Config> ;
//template class NonlinearOptimizer<Pose2Graph, Pose2Config> ; //template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
void Pose2Graph::print(const std::string& s) const { void Pose2Graph::print(const std::string& s) const {

5
cpp/gtsam.h
View File

@ -2,6 +2,7 @@ class Ordering {
Ordering(); Ordering();
void push_back(string s); void push_back(string s);
void print(string s) const; void print(string s) const;
void unique ();
}; };
class SymbolicFactor{ class SymbolicFactor{
@ -193,6 +194,8 @@ class Pose2Graph{
Pose2Graph(); Pose2Graph();
void print(string s) const; void print(string s) const;
bool equals(const Pose2Graph& p, double tol) const; bool equals(const Pose2Graph& p, double tol) const;
GaussianFactorGraph linearize(const Pose2Config& config) const; GaussianFactorGraph* linearize_(const Pose2Config& config) const;
void push_back(Pose2Factor* factor); void push_back(Pose2Factor* factor);
}; };

1
cpp/testGaussianFactorGraph.cpp
View File

@ -31,6 +31,7 @@ double tol=1e-4;
TEST( GaussianFactorGraph, equals ){ TEST( GaussianFactorGraph, equals ){
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
fg.print("fg");
GaussianFactorGraph fg2 = createGaussianFactorGraph(); GaussianFactorGraph fg2 = createGaussianFactorGraph();
CHECK(fg.equals(fg2)); CHECK(fg.equals(fg2));
} }