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

Updated interfaces and wrapped LinearContainerFactor

release/4.3a0
Alex Cunningham 2012-09-21 14:19:57 +00:00
parent 0ef12f2f20
commit 1985758d35
3 changed files with 56 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam_unstable/gtsam_unstable.h
View File

@ -10,6 +10,14 @@ virtual class gtsam::Pose2;
virtual class gtsam::Pose3; virtual class gtsam::Pose3;
virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Base;
virtual class gtsam::NonlinearFactor; virtual class gtsam::NonlinearFactor;
virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor;
class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph;
class gtsam::Ordering;
class gtsam::Values;
class gtsam::InvertedOrdering;
namespace gtsam { namespace gtsam {
@ -167,7 +175,46 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
}; };
//************************************************************************* //*************************************************************************
// General nonlinear factor types // nonlinear
//*************************************************************************
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
virtual class LinearContainerFactor : gtsam::NonlinearFactor {
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering,
const gtsam::Values& linearizationPoint);
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint);
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering,
const gtsam::Values& linearizationPoint);
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering);
LinearContainerFactor(gtsam::GaussianFactor* factor);
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering);
gtsam::GaussianFactor* factor() const;
// const boost::optional<Values>& linearizationPoint() const;
gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const;
gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const;
gtsam::NonlinearFactor* negate() const;
bool isJacobian() const;
gtsam::JacobianFactor* toJacobian() const;
gtsam::HessianFactor* toHessian() const;
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint);
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint);
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
const gtsam::Ordering& ordering);
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
const gtsam::InvertedOrdering& invOrdering);
}; // \class LinearContainerFactor
//*************************************************************************
// slam
//************************************************************************* //*************************************************************************
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>

9
gtsam_unstable/nonlinear/LinearContainerFactor.cpp
View File

@ -219,17 +219,20 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const {
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
const GaussianFactorGraph& linear_graph, const Ordering& ordering) { const GaussianFactorGraph& linear_graph, const Ordering& ordering,
const Values& linearizationPoint) {
return convertLinearGraph(linear_graph, ordering.invert()); return convertLinearGraph(linear_graph, ordering.invert());
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering) { const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering,
const Values& linearizationPoint) {
NonlinearFactorGraph result; NonlinearFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph)
if (f) if (f)
result.push_back(NonlinearFactorGraph::sharedFactor(new LinearContainerFactor(f, invOrdering))); result.push_back(NonlinearFactorGraph::sharedFactor(
new LinearContainerFactor(f, invOrdering, linearizationPoint)));
return result; return result;
} }

4
gtsam_unstable/nonlinear/LinearContainerFactor.h
View File

@ -132,10 +132,10 @@ public:
* If the inverse ordering is present, it will be faster. * If the inverse ordering is present, it will be faster.
*/ */
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
const Ordering& ordering); const Ordering& ordering, const Values& linearizationPoint = Values());
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
const InvertedOrdering& invOrdering); const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values());
protected: protected:
void rekeyFactor(const Ordering& ordering); void rekeyFactor(const Ordering& ordering);