diff --git a/gtsam.h b/gtsam.h index 7619c7854..9d1c0391e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1217,6 +1217,14 @@ virtual class JacobianFactor : gtsam::GaussianFactor { const gtsam::noiseModel::Diagonal* model); JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + size_t i4, Matrix A4, Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + size_t i4, Matrix A4, size_t i5, Matrix A5, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + size_t i4, Matrix A4, size_t i5, Matrix A5, size_t i6, Matrix A6, Vector b, + const gtsam::noiseModel::Diagonal* model); JacobianFactor(const gtsam::GaussianFactorGraph& graph); //Testable diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 0eb148d36..dd9664935 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -102,6 +102,34 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, b, model); } +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, + const Vector& b, const SharedDiagonal& model) { + fillTerms( + cref_list_of<4>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) + (make_pair(i4, A4)), b, model); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, + Key i5, const Matrix& A5, const Vector& b, const SharedDiagonal& model) { + fillTerms( + cref_list_of<5>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) + (make_pair(i4, A4))(make_pair(i5, A5)), b, model); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, + Key i5, const Matrix& A5, Key i6, const Matrix& A6, const Vector& b, + const SharedDiagonal& model) { + fillTerms( + cref_list_of<6>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) + (make_pair(i4, A4))(make_pair(i5, A5))(make_pair(i6, A6)), b, model); +} + /* ************************************************************************* */ JacobianFactor::JacobianFactor(const HessianFactor& factor) : Base(factor), Ab_( @@ -115,9 +143,6 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) : bool success; boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); - factor.print("HessianFactor to convert: "); - cout << "Maxrank: " << maxrank << ", success: " << int(success) << endl; - // Check for indefinite system if (!success) throw IndeterminantLinearSystemException(factor.keys().front()); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index e3c55feb2..ce2a47ab3 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -126,6 +126,22 @@ namespace gtsam { const Matrix& A2, Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + /** Construct four-ary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + + /** Construct five-ary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, + Key i5, const Matrix& A5, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + + /** Construct six-ary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, + Key i5, const Matrix& A5, Key i6, const Matrix& A6, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the * collection of keys and matrices making up the factor. */