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

constructors for JacobianFactor up to 6-ary for testing in matlab

release/4.3a0
thduynguyen 2014-08-05 21:58:05 -04:00
parent ecb896ef03
commit 3778e3c928
3 changed files with 52 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam.h
View File

@ -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

31
gtsam/linear/JacobianFactor.cpp
View File

@ -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());

16
gtsam/linear/JacobianFactor.h
View File

@ -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<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */