diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 077d6c4e7..8997b0678 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -10,6 +10,14 @@ virtual class gtsam::Pose2; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; 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 { @@ -167,7 +175,46 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { }; //************************************************************************* -// General nonlinear factor types +// nonlinear +//************************************************************************* +#include +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& 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 diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index b5701fb13..ee7e2eac1 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -219,17 +219,20 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { /* ************************************************************************* */ 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()); } /* ************************************************************************* */ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering) { + const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering, + const Values& linearizationPoint) { NonlinearFactorGraph result; BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) if (f) - result.push_back(NonlinearFactorGraph::sharedFactor(new LinearContainerFactor(f, invOrdering))); + result.push_back(NonlinearFactorGraph::sharedFactor( + new LinearContainerFactor(f, invOrdering, linearizationPoint))); return result; } diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 324556622..605a5885b 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -132,10 +132,10 @@ public: * If the inverse ordering is present, it will be faster. */ static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const Ordering& ordering); + const Ordering& ordering, const Values& linearizationPoint = Values()); static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const InvertedOrdering& invOrdering); + const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values()); protected: void rekeyFactor(const Ordering& ordering);