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

add generic optional parameters to sparseJacobian 

Also, the unit test changed due to a 0 entry that was previously wrongly
included in the b-column of the sparse representation.
release/4.3a0
Gerry Chen 2021-01-19 16:59:12 -05:00
parent ada83d92dc
commit 7a6f632f4c
4 changed files with 128 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/base/Matrix.h
View File

@ -33,6 +33,8 @@
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <vector>
/**
* Matrix is a typedef in the gtsam namespace
* TODO: make a version to work with matlab wrapping
@ -73,6 +75,12 @@ GTSAM_MAKE_MATRIX_DEFS(9);
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
/// Sparse matrix representation as vector of tuples.
/// See SparseMatrix.h for additional representations SparseMatrixEigenTriplets
/// and SparseMatrixEigen
typedef std::vector<boost::tuple<size_t, size_t, double>>
SparseMatrixBoostTriplets;
// Matrix formatting arguments when printing.
// Akin to Matlab style.
const Eigen::IOFormat& matlabFormat();

107
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -100,7 +100,9 @@ namespace gtsam {
}
/* ************************************************************************* */
vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
const Ordering& ordering, size_t& nrows, size_t& ncols) const {
gttic_(GaussianFactorGraph_sparseJacobian);
// First find dimensions of each variable
typedef std::map<Key, size_t> KeySizeMap;
KeySizeMap dims;
@ -108,24 +110,24 @@ namespace gtsam {
if (!static_cast<bool>(factor))
continue;
for (GaussianFactor::const_iterator key = factor->begin();
key != factor->end(); ++key) {
dims[*key] = factor->getDim(key);
for (auto it = factor->begin(); it != factor->end(); ++it) {
dims[*it] = factor->getDim(it);
}
}
// Compute first scalar column of each variable
size_t currentColIndex = 0;
ncols = 0;
KeySizeMap columnIndices = dims;
for (const KeySizeMap::value_type& col : dims) {
columnIndices[col.first] = currentColIndex;
currentColIndex += dims[col.first];
for (const auto key : ordering) {
columnIndices[key] = ncols;
ncols += dims[key];
}
// Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> entries;
size_t row = 0;
SparseMatrixBoostTriplets entries;
entries.reserve(60 * size());
nrows = 0;
for (const sharedFactor& factor : *this) {
if (!static_cast<bool>(factor)) continue;
@ -154,38 +156,79 @@ namespace gtsam {
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i, j);
if (std::abs(s) > 1e-12)
entries.emplace_back(row + i, column_start + j, s);
entries.emplace_back(nrows + i, column_start + j, s);
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = currentColIndex;
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));
for (size_t i = 0; i < (size_t) whitenedb.size(); i++) {
double s = whitenedb(i);
if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
}
// Increment row index
row += jacobianFactor->rows();
nrows += jacobianFactor->rows();
}
return vector<triplet>(entries.begin(), entries.end());
ncols++; // +1 for b-column
return entries;
}
/* ************************************************************************* */
SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
const Ordering& ordering) const {
size_t dummy1, dummy2;
return sparseJacobian(ordering, dummy1, dummy2);
}
/* ************************************************************************* */
SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
size_t& nrows, size_t& ncols) const {
return sparseJacobian(Ordering(this->keys()), nrows, ncols);
}
/* ************************************************************************* */
SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian() const {
size_t dummy1, dummy2;
return sparseJacobian(dummy1, dummy2);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_(
const Ordering& ordering, size_t& nrows, size_t& ncols) const {
gttic_(GaussianFactorGraph_sparseJacobian_matrix);
// call sparseJacobian
auto result = sparseJacobian(ordering, nrows, ncols);
// translate to base 1 matrix
size_t nzmax = result.size();
Matrix IJS(3, nzmax);
for (size_t k = 0; k < result.size(); k++) {
const auto& entry = result[k];
IJS(0, k) = double(entry.get<0>() + 1);
IJS(1, k) = double(entry.get<1>() + 1);
IJS(2, k) = entry.get<2>();
}
return IJS;
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_(
const Ordering& ordering) const {
size_t dummy1, dummy2;
return sparseJacobian_(ordering, dummy1, dummy2);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_(
size_t& nrows, size_t& ncols) const {
return sparseJacobian_(Ordering(this->keys()), nrows, ncols);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_() const {
// call sparseJacobian
typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> result = sparseJacobian();
// translate to base 1 matrix
size_t nzmax = result.size();
Matrix IJS(3,nzmax);
for (size_t k = 0; k < result.size(); k++) {
const triplet& entry = result[k];
IJS(0,k) = double(entry.get<0>() + 1);
IJS(1,k) = double(entry.get<1>() + 1);
IJS(2,k) = entry.get<2>();
}
return IJS;
size_t dummy1, dummy2;
return sparseJacobian_(dummy1, dummy2);
}
/* ************************************************************************* */

46
gtsam/linear/GaussianFactorGraph.h
View File

@ -181,17 +181,51 @@ namespace gtsam {
///@{
/**
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
* where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
* The standard deviations are baked into A and b
* Return vector of i, j, and s to generate an m-by-n sparse augmented
* Jacobian matrix, where i(k) and j(k) are the base 0 row and column
* indices, s(k) a double. The standard deviations are baked into A and b
* @param ordering the column ordering
* @param[out] nrows The number of rows in the Jacobian
* @param[out] ncols The number of columns in the Jacobian
* @return the sparse matrix in one of the 4 forms above
*/
std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
SparseMatrixBoostTriplets sparseJacobian(const Ordering& ordering,
size_t& nrows,
size_t& ncols) const;
/// Returns a sparse augmented Jacobian without outputting its dimensions
SparseMatrixBoostTriplets sparseJacobian(
const Ordering& ordering) const;
/// Returns a sparse augmented Jacobian with default Ordering
SparseMatrixBoostTriplets sparseJacobian(size_t& nrows,
size_t& ncols) const;
/// Returns a sparse augmented Jacobian without with default ordering and
/// outputting its dimensions
SparseMatrixBoostTriplets sparseJacobian() const;
/**
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
* entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's
* sparse. Note: i, j are 1-indexed.
* The standard deviations are baked into A and b
*/
Matrix sparseJacobian_(const Ordering& ordering,
size_t& nrows,
size_t& ncols) const;
/// Returns a matrix-form sparse augmented Jacobian without outputting its
/// dimensions
Matrix sparseJacobian_(
const Ordering& ordering) const;
/// Returns a matrix-form sparse augmented Jacobian with default Ordering
Matrix sparseJacobian_(size_t& nrows,
size_t& ncols) const;
/// Returns a matrix-form sparse augmented Jacobian with default ordering
/// and without outputting its dimensions
Matrix sparseJacobian_() const;
/**

9
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -66,10 +66,11 @@ TEST(GaussianFactorGraph, initialization) {
// Test sparse, which takes a vector and returns a matrix, used in MATLAB
// Note that this the augmented vector and the RHS is in column 7
Matrix expectedIJS =
(Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7.,
8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6.,
7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5.,
-5., 5., 5., -1., 1.5).finished();
(Matrix(3, 21) <<
1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5.,
1., -5., -5., 5., 5., -1., 1.5).finished();
Matrix actualIJS = fg.sparseJacobian_();
EQUALITY(expectedIJS, actualIJS);
}